pax_global_header00006660000000000000000000000064120774244230014517gustar00rootroot0000000000000052 comment=b42f71b33f8fa5e35859bdaa1135ed0610f9c86b choreonoid-1.1.0+dfsg/000077500000000000000000000000001207742442300145665ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/CMakeLists.txt000066400000000000000000000417501207742442300173350ustar00rootroot00000000000000 # @author Shin'ichiro Nakaoka cmake_minimum_required(VERSION 2.6) cmake_policy(SET CMP0003 NEW) set(CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS true) project(Choreonoid) # set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules/) set(CNOID_MAJOR_VERSION 1) set(CNOID_MINOR_VERSION 1) set(CNOID_PATCH_VERSION 0) set(CNOID_VERSION ${CNOID_MAJOR_VERSION}.${CNOID_MINOR_VERSION}) set(CNOID_FULL_VERSION ${CNOID_MAJOR_VERSION}.${CNOID_MINOR_VERSION}.${CNOID_PATCH_VERSION}) include(FindPkgConfig) include(CheckIncludeFiles) include(FindPythonLibs) if(NOT CMAKE_BUILD_TYPE) set( CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." FORCE) endif() set(ADDITIONAL_CXX_FLAGS_RELEASE ${ADDITIONAL_CXX_FLAGS_RELEASE} CACHE STRING "Additional c++ compiler optimization flags") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${EXTRA_CXX_FLAGS_RELEASE} ${ADDITIONAL_CXX_FLAGS_RELEASE}") if(UNIX) set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -finline-functions") endif() if(MSVC) set(ext_compiler_options "/Ob2 /Ox /Oi /Ot /Oy /GT /GS- /arch:SSE /arch:SSE2 /fp:fast") set(ext_linker_options "") option(MSVC_ENABLE_GLOBAL_OPTIMIZATION "Global optimization with compiler option /GL and linker option /LTCG" ON) if(MSVC_ENABLE_GLOBAL_OPTIMIZATION) set(ext_compiler_options "${ext_compiler_options} /GL") set(ext_linker_options "${ext_linker_options} /LTCG") endif() set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${ext_compiler_options}") set(CMAKE_MODULE_LINKER_FLAGS_RELEASE "${CMAKE_MODULE_LINKER_FLAGS_RELEASE} ${ext_linker_options}") set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} ${ext_linker_options}") set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} ${ext_linker_options}") endif() if(WIN32) set(DEFAULT_INSTALL_SDK OFF) set(DEFAULT_INSTALL_DEPENDENCIES ON) else() set(DEFAULT_INSTALL_SDK ON) set(DEFAULT_INSTALL_DEPENDENCIES OFF) endif() option(INSTALL_SDK "Installs SDK files such as header files" ${DEFAULT_INSTALL_SDK}) option(INSTALL_DEPENDENCIES "Installs the external libraries" ${DEFAULT_INSTALL_DEPENDENCIES}) option(ENABLE_INSTALL_RPATH "Enable RPATH setting for installed binary files" OFF) if(ENABLE_INSTALL_RPATH) SET(CMAKE_SKIP_BUILD_RPATH FALSE) SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) #SET(CMAKE_INSTALL_RPATH "$ORIGIN/../lib") SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH FALSE) else() SET(CMAKE_SKIP_BUILD_RPATH FALSE) SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) SET(CMAKE_INSTALL_RPATH "") SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH FALSE) endif() # commands if(UNIX) set(RMDIR rm -fr) elseif(WIN32) set(RMDIR rmdir /S/Q) endif() # check "dlfcn.h" for using dlopen() and dlclose() if(UNIX) check_include_files(dlfcn.h HAVE_DLFCN_H) if(NOT HAVE_DLFCN_H) message(FATAL_ERROR "Could not find dlfcn.h") endif() endif() # gettext option(CNOID_ENABLE_GETTEXT "Enable the gettext library and translation messages for the internationalization" ON) if(CNOID_ENABLE_GETTEXT) if(WIN32) FIND_PROGRAM(GETTEXT_MSGFMT_EXECUTABLE msgfmt ${PROJECT_SOURCE_DIR}/thirdparty/windows/bin) else() FIND_PROGRAM(GETTEXT_MSGFMT_EXECUTABLE msgfmt) endif() if(NOT GETTEXT_MSGFMT_EXECUTABLE) message(FATAL_ERROR "Could not find the msgfmt command and gettext cannot be enabled.") endif() get_filename_component(GETTEXT_BINARY_DIR ${GETTEXT_MSGFMT_EXECUTABLE} PATH) get_filename_component(GETTEXT_DIR ${GETTEXT_BINARY_DIR} PATH) set(GETTEXT_INCLUDE_DIR ${GETTEXT_DIR}/include) set(GETTEXT_LIBRARY_DIR ${GETTEXT_DIR}/lib) include_directories(${GETTEXT_INCLUDE_DIR}) link_directories(${GETTEXT_LIBRARY_DIR}) if(WIN32) set(GETTEXT_LIBRARIES intl) else() set(GETTEXT_LIBRARIES "") endif() else() set(GETTEXT_LIBRARIES "") endif() # boost set(Boost_USE_STATIC_LIBS OFF) set(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.43", "1.43.0", "1.44", "1.44.0", "1.45", "1.45.0", "1.46", "1.46.0" "1.47" "1.47.0") find_package(Boost 1.36.0 REQUIRED COMPONENTS system filesystem program_options regex signals thread iostreams date_time python) if(NOT Boost_FOUND) set(BOOST_ROOT ${BOOST_ROOT} CACHE PATH "set the directory of the boost library") message(FATAL_ERROR "Boost cannot be found. Please specify the boost top directory to BOOST_ROOT.") endif() include_directories(${Boost_INCLUDE_DIRS}) link_directories(${Boost_LIBRARY_DIRS}) set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS_RELEASE BOOST_DISABLE_ASSERTS) # todo: update filesystem version to 3 set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS BOOST_FILESYSTEM_VERSION=2) if(MSVC) set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS BOOST_ALL_DYN_LINK ${BOOST_LIB_DIAGNOSTIC}) #MSVC 2010 problem if(MSVC_VERSION EQUAL 1600) #add_definitions(-D) # for VC++2010 Express Edition IF(NOT DEFINED CMAKE_INSTALL_SYSTEM_RUNTIME_LIBS_NO_WARNINGS) SET(CMAKE_INSTALL_SYSTEM_RUNTIME_LIBS_NO_WARNINGS ON) ENDIF() set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS _HAS_CPP0X=0 _ITERATOR_DEBUG_LEVEL=0 # needed to use boost.MultiArray with the Visual C++ 2010 Debug mode BOOST_NO_0X_HDR_TYPEINDEX BOOST_NO_0X_HDR_TUPLE ) endif() endif() # eigen option(USE_EXTERNAL_EIGEN "Use the eigen library installed in an external directory" OFF) if(USE_EXTERNAL_EIGEN) if(UNIX) pkg_check_modules(EIGEN REQUIRED eigen3) elseif(MSVC) set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} CACHE PATH "set the header directory of the Eigen library") endif() include_directories(${EIGEN_INCLUDE_DIRS}) else() include_directories(${PROJECT_SOURCE_DIR}/thirdparty/eigen3) endif() set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS EIGEN_NO_DEBUG) if(MSVC) set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS EIGEN_DONT_ALIGN) endif() # qt find_package(Qt4 REQUIRED) set(QT_USE_QTOPENGL TRUE) #set(QT_USE_QTTEST TRUE) include(${QT_USE_FILE}) include_directories(${QT_INCLUDE_DIR} ${QT_OPENGL_INCLUDE_DIR}) add_definitions(-DQT_NO_KEYWORDS) # OpenSceneGraph set(OSG_DIR ${OSG_DIR} CACHE PATH "set the directory of the OpenSceneGraph library") if(MSVC) if(NOT OSG_DIR) message(FATAL_ERROR "Please specify the OpenSceneGraph top directory to OSG_DIR.") endif() endif() if(OSG_DIR) include_directories(${OSG_DIR}/include) link_directories(${OSG_DIR}/lib) endif() # libyaml if(UNIX) set(USE_EXTERNAL_YAML_DEFAULT ON) elseif(MSVC) set(USE_EXTERNAL_YAML_DEFAULT OFF) endif() option(USE_EXTERNAL_YAML "Use the yaml library installed in an external directory" ${USE_EXTERNAL_YAML_DEFAULT}) if(USE_EXTERNAL_YAML) set(LIBYAML_DIR CACHE PATH "set the top directory of the libyaml") if(LIBYAML_DIR) include_directories(${LIBYAML_DIR}/include) link_directories(${LIBYAML_DIR}/lib) endif() else() include_directories(${Choreonoid_SOURCE_DIR}/thirdparty/yaml-0.1.3/include) endif() if(UNIX) # png find_package(PNG REQUIRED) include_directories(${PNG_INCLUDE_DIR}) # jpeg find_package(JPEG REQUIRED) include_directories(${JPEG_INCLUDE_DIR}) elseif(MSVC) set(PNG_LIBRARY libpng) set(JPEG_LIBRARY jpeg) endif() # doxygen find_package(Doxygen) set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS_DEBUG CNOID_DEBUG) if(UNIX) option(PUT_ALL_WARNINGS "put all warnings in compile" OFF) if(PUT_ALL_WARNINGS) list(APPEND CMAKE_C_FLAGS "-Wall") list(APPEND CMAKE_CXX_FLAGS "-Wall") endif() option(CHECK_UNRESOLVED_SYMBOLS "check unresolved symbols in the object files when creating shared libraries" OFF) #mark_as_advanced(CHECK_UNRESOLVED_SYMBOLS) if(CHECK_UNRESOLVED_SYMBOLS) list(APPEND CMAKE_SHARED_LINKER_FLAGS "-Wl,--unresolved-symbols=ignore-in-shared-libs -Wl,--warn-unresolved-symbols") endif() elseif(MSVC) set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS _CRT_SECURE_NO_WARNINGS) set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS NOMINMAX _USE_MATH_DEFINES) #list(APPEND CMAKE_CXX_FLAGS "/wd4250 /wd4251 /wd4275") endif() include_directories(${PROJECT_SOURCE_DIR}) include_directories(${PROJECT_SOURCE_DIR}/include) set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin) set(CNOID_DIR ${CMAKE_INSTALL_PREFIX}) set(CNOID_SUBDIR choreonoid-${CNOID_VERSION}) set(CNOID_PLUGIN_SUBDIR lib/${CNOID_SUBDIR}) if(WIN32) set(CNOID_HEADER_SUBDIR "include") set(CNOID_SHARE_SUBDIR "share") set(CNOID_DOC_SUBDIR "share/doc") else() set(CNOID_HEADER_SUBDIR "include/${CNOID_SUBDIR}") set(CNOID_SHARE_SUBDIR "share/${CNOID_SUBDIR}") set(CNOID_DOC_SUBDIR "share/doc/${CNOID_SUBDIR}") endif() set(CNOID_SHARE_DIR "${CNOID_DIR}/${CNOID_SHARE_SUBDIR}") set(CNOID_LOCALE_SUBDIR "share/locale") link_directories(${PROJECT_BINARY_DIR}/lib) link_directories(${PROJECT_BINARY_DIR}/${CNOID_PLUGIN_SUBDIR}) # Document installaiton install(FILES NEWS DESTINATION ${CNOID_DOC_SUBDIR}) install(FILES LICENSE DESTINATION ${CNOID_DOC_SUBDIR}) if(MSVC) include_directories(${PROJECT_SOURCE_DIR}/thirdparty/windows/include) link_directories(${PROJECT_SOURCE_DIR}/thirdparty/windows/lib) endif() #add_subdirectory(thirdparty) function(apply_common_setting_for_target target) set(headers ${ARGV1}) if(MSVC) if(MSVC_IDE) if(headers) source_group("Header Files" FILES ${headers}) endif() #set_target_properties(${target} PROPERTIES PREFIX "../" IMPORT_PREFIX "../") endif() get_target_property(existing_compile_flags ${target} COMPILE_FLAGS) if(existing_compile_flags STREQUAL existing_compile_flags-NOTFOUND) set(existing_compile_flags "") endif() set_target_properties(${target} PROPERTIES COMPILE_FLAGS "${existing_compile_flags} /MP /wd4250 /wd4251 /wd4275 /wd4819") get_target_property(existing_link_flags ${target} LINK_FLAGS) if(existing_link_flags STREQUAL existing_link_flags-NOTFOUND) set(existing_link_flags "") endif() set_target_properties(${target} PROPERTIES LINK_FLAGS "${existing_link_flags} /NODEFAULTLIB:LIBCMT") set_target_properties(${target} PROPERTIES DEBUG_POSTFIX d) endif() endfunction() function(apply_common_setting_for_library target) set(headers ${ARGV1}) set_target_properties(${target} PROPERTIES VERSION ${CNOID_VERSION}) apply_common_setting_for_target(${target} "${headers}") if(INSTALL_SDK) install(TARGETS ${target} RUNTIME DESTINATION bin CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel LIBRARY DESTINATION lib CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel ARCHIVE DESTINATION lib CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel) if(headers) get_filename_component(subdir ${CMAKE_CURRENT_SOURCE_DIR} NAME_WE) install(FILES ${headers} DESTINATION ${CNOID_HEADER_SUBDIR}/cnoid/src/${subdir}) endif() else() install(TARGETS ${target} RUNTIME DESTINATION bin CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel LIBRARY DESTINATION lib CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel) endif() endfunction() function(apply_common_setting_for_plugin target) set(headers ${ARGV1}) apply_common_setting_for_target(${target} "${headers}") if(INSTALL_SDK) install(TARGETS ${target} RUNTIME DESTINATION ${CNOID_PLUGIN_SUBDIR} CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel LIBRARY DESTINATION ${CNOID_PLUGIN_SUBDIR} CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel ARCHIVE DESTINATION ${CNOID_PLUGIN_SUBDIR} CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel) if(headers) get_filename_component(subdir ${CMAKE_CURRENT_SOURCE_DIR} NAME_WE) install(FILES ${headers} DESTINATION ${CNOID_HEADER_SUBDIR}/cnoid/src/${subdir}) endif() else() install(TARGETS ${target} RUNTIME DESTINATION ${CNOID_PLUGIN_SUBDIR} CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel LIBRARY DESTINATION ${CNOID_PLUGIN_SUBDIR} CONFIGURATIONS Release Debug RelWithDebInfo MinSizeRel) endif() endfunction() function(make_gettext_mofiles out_mofiles) configure_file(${PROJECT_SOURCE_DIR}/src/Util/gettext.h.in ${CMAKE_CURRENT_SOURCE_DIR}/gettext.h) if(NOT CNOID_ENABLE_GETTEXT) set(${out_mofiles} "") return() endif() file(GLOB pofiles ${CMAKE_CURRENT_SOURCE_DIR}/po/*.po) foreach(pofile ${pofiles}) get_filename_component(lang ${pofile} NAME_WE) set(message_location share/locale/${lang}/LC_MESSAGES) file(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/${message_location}) set(mofile ${PROJECT_BINARY_DIR}/${message_location}/${target}-${CNOID_VERSION}.mo) add_custom_command( OUTPUT ${mofile} COMMAND ${GETTEXT_MSGFMT_EXECUTABLE} -o ${mofile} ${pofile} DEPENDS ${pofile} ) list(APPEND mofiles ${mofile}) install(FILES ${mofile} DESTINATION "${CNOID_LOCALE_SUBDIR}/${lang}/LC_MESSAGES") endforeach() set(${out_mofiles} ${mofiles} PARENT_SCOPE) endfunction() function(install_external_libraries dll_dir lib_dir) set(libraries ${ARGV}) list(REMOVE_AT libraries 0 1) if(INSTALL_DEPENDENCIES AND MSVC) set(conf general) foreach(library ${libraries}) #message(STATUS ${library}) if(library STREQUAL general) set(conf general) elseif(library STREQUAL optimized) set(conf optimized) elseif(library STREQUAL debug) set(conf debug) else() get_filename_component(filename ${library} NAME_WE) if(conf STREQUAL general) if(dll_dir) install(PROGRAMS ${dll_dir}/${filename}.dll DESTINATION bin) endif() if(lib_dir AND INSTALL_SDK) install(PROGRAMS ${lib_dir}/${filename}.lib DESTINATION lib) endif() elseif(conf STREQUAL optimized) if(dll_dir) install(PROGRAMS ${dll_dir}/${filename}.dll DESTINATION bin CONFIGURATIONS Release) endif() if(lib_dir AND INSTALL_SDK) install(PROGRAMS ${lib_dir}/${filename}.lib DESTINATION lib CONFIGURATIONS Release) endif() elseif(conf STREQUAL debug) if(dll_dir) install(PROGRAMS ${dll_dir}/${filename}.dll DESTINATION bin CONFIGURATIONS Debug) endif() if(lib_dir AND INSTALL_SDK) install(PROGRAMS ${lib_dir}/${filename}.lib DESTINATION lib CONFIGURATIONS Debug) endif() endif() endif() endforeach() endif() endfunction() add_subdirectory(src) add_subdirectory(include) add_subdirectory(extplugin) add_subdirectory(share) if(EXISTS ${PROJECT_SOURCE_DIR}/admin) add_subdirectory(admin) endif() configure_file(Doxyfile.in ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile @ONLY) if(EXISTS ${PROJECT_SOURCE_DIR}/test) if(EXISTS ${PROJECT_SOURCE_DIR}/test/CMakeLists.txt) add_subdirectory(test) endif() endif() # CPack include(InstallRequiredSystemLibraries) set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Choreonoid") set(CPACK_PACKAGE_VENDOR "Shin'ichiro Nakaoka, AIST") #set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/ReadMe.txt") set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") set(CPACK_PACKAGE_VERSION_MAJOR ${CNOID_MAJOR_VERSION}) set(CPACK_PACKAGE_VERSION_MINOR ${CNOID_MINOR_VERSION}) set(CPACK_PACKAGE_VERSION_PATCH ${CNOID_PATCH_VERSION}) set(CPACK_PACKAGE_INSTALL_DIRECTORY "Choreonoid ${CNOID_VERSION}") set(CPACK_PACKAGE_INSTALL_REGISTORY_KEY "Choreonoid ${CNOID_VERSION}") set(CPACK_GENERATOR NSIS) if(WIN32) set(CPACK_GENERATOR NSIS) set(CPACK_PACKAGE_ICON "${CMAKE_SOURCE_DIR}\\\\src\\\\Choreonoid\\\\icon\\\\choreonoid.ico") set(CPACK_NSIS_INSTALLED_ICON_NAME "bin\\\\choreonoid.exe") set(CPACK_NSIS_DISPLAY_NAME "Choreonoid ${CNOID_VERSION}") set(CPACK_NSIS_CONTACT "choreonoid@m.aist.go.jp") set(CPACK_NSIS_MODIFY_PATH ON) set(CPACK_PACKAGE_EXECUTABLES "choreonoid;Choreonoid") set(CPACK_CREATE_DESKTOP_LINKS choreonoid) set(CPACK_NSIS_URL_INFO_ABOUT "http://choreonoid.org") set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS "!include \\\"FileAssociation.nsh\\\"\n\\\${registerExtension} \\\"\$INSTDIR\\\\bin\\\\choreonoid.exe\\\" \\\".cnoid\\\" \\\"ChoreonoidProject\\\" \nSystem::Call 'shell32.dll::SHChangeNotify(i, i, i, i) v (0x08000000, 0, 0, 0)'") set(CPACK_NSIS_EXTRA_UNINSTALL_COMMANDS "\\\${unregisterExtension} \\\".cnoid\\\" \\\"ChoreonoidProject\\\" \nSystem::Call 'shell32.dll::SHChangeNotify(i, i, i, i) v (0x08000000, 0, 0, 0)'") elseif(UNIX) set(CPACK_GENERATOR DEB) set(CPACK_PACKAGE_FILE_NAME "choreonoid_${CNOID_FULL_VERSION}_i386") set(CPACK_DEBIAN_PACKAGE_DEPENDS "libc6, libgcc1, libstdc++6, libpng12-0, libjpeg62, libyaml-0-2, zlib1g, libboost-filesystem1.42.0, libboost-date-time1.42.0, libboost-program-options1.42.0, libboost-python1.42.0, libboost-regex1.42.0, libboost-signals1.42.0, libboost-system1.42.0, libboost-thread1.42.0, libqtcore4, libqtgui4, libqt4-test, libqt4-opengl, libopenscenegraph65") if(INSTALL_SDK) set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libstdc++-dev, libboost-dev, libboost-program-options-dev, libboost-python-dev, libboost-signals-dev, libqt4-dev, libqt4-opengl-dev, libopenscenegraph-dev") endif() set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Shin'ichiro Nakaoka") endif() #set(CPACK_INCLUDE_TOPLEVEL_DIRECTORY 1) include(CPack) add_subdirectory(misc) choreonoid-1.1.0+dfsg/Doxyfile.in000066400000000000000000001742441207742442300167150ustar00rootroot00000000000000# Doxyfile 1.5.8 # This file describes the settings to be used by the documentation system # doxygen (www.doxygen.org) for a project # # All text after a hash (#) is considered a comment and will be ignored # The format is: # TAG = value [value, ...] # For lists items can also be appended using: # TAG += value [value, ...] # Values that contain spaces should be placed between quotes (" ") #--------------------------------------------------------------------------- # Project related configuration options #--------------------------------------------------------------------------- # This tag specifies the encoding used for all characters in the config file # that follow. The default is UTF-8 which is also the encoding used for all # text before the first occurrence of this tag. Doxygen uses libiconv (or the # iconv built into libc) for the transcoding. See # http://www.gnu.org/software/libiconv for the list of possible encodings. DOXYFILE_ENCODING = UTF-8 # The PROJECT_NAME tag is a single word (or a sequence of words surrounded # by quotes) that should identify the project. PROJECT_NAME = "Choreonoid" # The PROJECT_NUMBER tag can be used to enter a project or revision number. # This could be handy for archiving the generated documentation or # if some version control system is used. PROJECT_NUMBER = @CNOID_VERSION@ # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) # base path where the generated documentation will be put. # If a relative path is entered, it will be relative to the location # where doxygen was started. If left blank the current directory will be used. OUTPUT_DIRECTORY = . # If the CREATE_SUBDIRS tag is set to YES, then doxygen will create # 4096 sub-directories (in 2 levels) under the output directory of each output # format and will distribute the generated files over these directories. # Enabling this option can be useful when feeding doxygen a huge amount of # source files, where putting all generated files in the same directory would # otherwise cause performance problems for the file system. CREATE_SUBDIRS = NO # The OUTPUT_LANGUAGE tag is used to specify the language in which all # documentation generated by doxygen is written. Doxygen will use this # information to generate all constant output in the proper language. # The default language is English, other supported languages are: # Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, # Croatian, Czech, Danish, Dutch, Farsi, Finnish, French, German, Greek, # Hungarian, Italian, Japanese, Japanese-en (Japanese with English messages), # Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, Polish, # Portuguese, Romanian, Russian, Serbian, Serbian-Cyrilic, Slovak, Slovene, # Spanish, Swedish, and Ukrainian. #OUTPUT_LANGUAGE = English OUTPUT_LANGUAGE = Japanese # If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will # include brief member descriptions after the members that are listed in # the file and class documentation (similar to JavaDoc). # Set to NO to disable this. BRIEF_MEMBER_DESC = YES # If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend # the brief description of a member or function before the detailed description. # Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the # brief descriptions will be completely suppressed. REPEAT_BRIEF = YES # This tag implements a quasi-intelligent brief description abbreviator # that is used to form the text in various listings. Each string # in this list, if found as the leading text of the brief description, will be # stripped from the text and the result after processing the whole list, is # used as the annotated text. Otherwise, the brief description is used as-is. # If left blank, the following values are used ("$name" is automatically # replaced with the name of the entity): "The $name class" "The $name widget" # "The $name file" "is" "provides" "specifies" "contains" # "represents" "a" "an" "the" ABBREVIATE_BRIEF = # If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then # Doxygen will generate a detailed section even if there is only a brief # description. ALWAYS_DETAILED_SEC = NO # If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all # inherited members of a class in the documentation of that class as if those # members were ordinary class members. Constructors, destructors and assignment # operators of the base classes will not be shown. INLINE_INHERITED_MEMB = NO # If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full # path before files name in the file list and in the header files. If set # to NO the shortest path that makes the file name unique will be used. FULL_PATH_NAMES = NO # If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag # can be used to strip a user-defined part of the path. Stripping is # only done if one of the specified strings matches the left-hand part of # the path. The tag can be used to show relative paths in the file list. # If left blank the directory from which doxygen is run is used as the # path to strip. STRIP_FROM_PATH = # The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of # the path mentioned in the documentation of a class, which tells # the reader which header file to include in order to use a class. # If left blank only the name of the header file containing the class # definition is used. Otherwise one should specify the include paths that # are normally passed to the compiler using the -I flag. STRIP_FROM_INC_PATH = # If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter # (but less readable) file names. This can be useful is your file systems # doesn't support long names like on DOS, Mac, or CD-ROM. SHORT_NAMES = NO # If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen # will interpret the first line (until the first dot) of a JavaDoc-style # comment as the brief description. If set to NO, the JavaDoc # comments will behave just like regular Qt-style comments # (thus requiring an explicit @brief command for a brief description.) JAVADOC_AUTOBRIEF = NO # If the QT_AUTOBRIEF tag is set to YES then Doxygen will # interpret the first line (until the first dot) of a Qt-style # comment as the brief description. If set to NO, the comments # will behave just like regular Qt-style comments (thus requiring # an explicit \brief command for a brief description.) QT_AUTOBRIEF = NO # The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen # treat a multi-line C++ special comment block (i.e. a block of //! or /// # comments) as a brief description. This used to be the default behaviour. # The new default is to treat a multi-line C++ comment block as a detailed # description. Set this tag to YES if you prefer the old behaviour instead. MULTILINE_CPP_IS_BRIEF = NO # If the INHERIT_DOCS tag is set to YES (the default) then an undocumented # member inherits the documentation from any documented member that it # re-implements. INHERIT_DOCS = YES # If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce # a new page for each member. If set to NO, the documentation of a member will # be part of the file/class/namespace that contains it. SEPARATE_MEMBER_PAGES = NO # The TAB_SIZE tag can be used to set the number of spaces in a tab. # Doxygen uses this value to replace tabs by spaces in code fragments. TAB_SIZE = 8 # This tag can be used to specify a number of aliases that acts # as commands in the documentation. An alias has the form "name=value". # For example adding "sideeffect=\par Side Effects:\n" will allow you to # put the command \sideeffect (or @sideeffect) in the documentation, which # will result in a user-defined paragraph with heading "Side Effects:". # You can put \n's in the value part of an alias to insert newlines. ALIASES = # Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C # sources only. Doxygen will then generate output that is more tailored for C. # For instance, some of the names that are used will be different. The list # of all members will be omitted, etc. OPTIMIZE_OUTPUT_FOR_C = NO # Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java # sources only. Doxygen will then generate output that is more tailored for # Java. For instance, namespaces will be presented as packages, qualified # scopes will look different, etc. OPTIMIZE_OUTPUT_JAVA = NO # Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran # sources only. Doxygen will then generate output that is more tailored for # Fortran. OPTIMIZE_FOR_FORTRAN = NO # Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL # sources. Doxygen will then generate output that is tailored for # VHDL. OPTIMIZE_OUTPUT_VHDL = NO # Doxygen selects the parser to use depending on the extension of the files it parses. # With this tag you can assign which parser to use for a given extension. # Doxygen has a built-in mapping, but you can override or extend it using this tag. # The format is ext=language, where ext is a file extension, and language is one of # the parsers supported by doxygen: IDL, Java, Javascript, C#, C, C++, D, PHP, # Objective-C, Python, Fortran, VHDL, C, C++. For instance to make doxygen treat # .inc files as Fortran files (default is PHP), and .f files as C (default is Fortran), # use: inc=Fortran f=C EXTENSION_MAPPING = # If you use STL classes (i.e. std::string, std::vector, etc.) but do not want # to include (a tag file for) the STL sources as input, then you should # set this tag to YES in order to let doxygen match functions declarations and # definitions whose arguments contain STL classes (e.g. func(std::string); v.s. # func(std::string) {}). This also make the inheritance and collaboration # diagrams that involve STL classes more complete and accurate. BUILTIN_STL_SUPPORT = YES # If you use Microsoft's C++/CLI language, you should set this option to YES to # enable parsing support. CPP_CLI_SUPPORT = NO # Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. # Doxygen will parse them like normal C++ but will assume all classes use public # instead of private inheritance when no explicit protection keyword is present. SIP_SUPPORT = NO # For Microsoft's IDL there are propget and propput attributes to indicate getter # and setter methods for a property. Setting this option to YES (the default) # will make doxygen to replace the get and set methods by a property in the # documentation. This will only work if the methods are indeed getting or # setting a simple type. If this is not the case, or you want to show the # methods anyway, you should set this option to NO. IDL_PROPERTY_SUPPORT = YES # If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC # tag is set to YES, then doxygen will reuse the documentation of the first # member in the group (if any) for the other members of the group. By default # all members of a group must be documented explicitly. DISTRIBUTE_GROUP_DOC = NO # Set the SUBGROUPING tag to YES (the default) to allow class member groups of # the same type (for instance a group of public functions) to be put as a # subgroup of that type (e.g. under the Public Functions section). Set it to # NO to prevent subgrouping. Alternatively, this can be done per class using # the \nosubgrouping command. SUBGROUPING = YES # When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum # is documented as struct, union, or enum with the name of the typedef. So # typedef struct TypeS {} TypeT, will appear in the documentation as a struct # with name TypeT. When disabled the typedef will appear as a member of a file, # namespace, or class. And the struct will be named TypeS. This can typically # be useful for C code in case the coding convention dictates that all compound # types are typedef'ed and only the typedef is referenced, never the tag name. TYPEDEF_HIDES_STRUCT = NO # The SYMBOL_CACHE_SIZE determines the size of the internal cache use to # determine which symbols to keep in memory and which to flush to disk. # When the cache is full, less often used symbols will be written to disk. # For small to medium size projects (<1000 input files) the default value is # probably good enough. For larger projects a too small cache size can cause # doxygen to be busy swapping symbols to and from disk most of the time # causing a significant performance penality. # If the system has enough physical memory increasing the cache will improve the # performance by keeping more symbols in memory. Note that the value works on # a logarithmic scale so increasing the size by one will rougly double the # memory usage. The cache size is given by this formula: # 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, # corresponding to a cache size of 2^16 = 65536 symbols SYMBOL_CACHE_SIZE = 0 #--------------------------------------------------------------------------- # Build related configuration options #--------------------------------------------------------------------------- # If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in # documentation are documented, even if no documentation was available. # Private class members and static file members will be hidden unless # the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES EXTRACT_ALL = YES # If the EXTRACT_PRIVATE tag is set to YES all private members of a class # will be included in the documentation. EXTRACT_PRIVATE = NO # If the EXTRACT_STATIC tag is set to YES all static members of a file # will be included in the documentation. EXTRACT_STATIC = NO # If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) # defined locally in source files will be included in the documentation. # If set to NO only classes defined in header files are included. EXTRACT_LOCAL_CLASSES = NO # This flag is only useful for Objective-C code. When set to YES local # methods, which are defined in the implementation section but not in # the interface are included in the documentation. # If set to NO (the default) only methods in the interface are included. EXTRACT_LOCAL_METHODS = NO # If this flag is set to YES, the members of anonymous namespaces will be # extracted and appear in the documentation as a namespace called # 'anonymous_namespace{file}', where file will be replaced with the base # name of the file that contains the anonymous namespace. By default # anonymous namespace are hidden. EXTRACT_ANON_NSPACES = NO # If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all # undocumented members of documented classes, files or namespaces. # If set to NO (the default) these members will be included in the # various overviews, but no documentation section is generated. # This option has no effect if EXTRACT_ALL is enabled. HIDE_UNDOC_MEMBERS = NO # If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all # undocumented classes that are normally visible in the class hierarchy. # If set to NO (the default) these classes will be included in the various # overviews. This option has no effect if EXTRACT_ALL is enabled. HIDE_UNDOC_CLASSES = NO # If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all # friend (class|struct|union) declarations. # If set to NO (the default) these declarations will be included in the # documentation. HIDE_FRIEND_COMPOUNDS = NO # If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any # documentation blocks found inside the body of a function. # If set to NO (the default) these blocks will be appended to the # function's detailed documentation block. HIDE_IN_BODY_DOCS = NO # The INTERNAL_DOCS tag determines if documentation # that is typed after a \internal command is included. If the tag is set # to NO (the default) then the documentation will be excluded. # Set it to YES to include the internal documentation. INTERNAL_DOCS = NO # If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate # file names in lower-case letters. If set to YES upper-case letters are also # allowed. This is useful if you have classes or files whose names only differ # in case and if your file system supports case sensitive file names. Windows # and Mac users are advised to set this option to NO. CASE_SENSE_NAMES = YES # If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen # will show members with their full class and namespace scopes in the # documentation. If set to YES the scope will be hidden. HIDE_SCOPE_NAMES = NO # If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen # will put a list of the files that are included by a file in the documentation # of that file. SHOW_INCLUDE_FILES = YES # If the INLINE_INFO tag is set to YES (the default) then a tag [inline] # is inserted in the documentation for inline members. INLINE_INFO = YES # If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen # will sort the (detailed) documentation of file and class members # alphabetically by member name. If set to NO the members will appear in # declaration order. SORT_MEMBER_DOCS = YES # If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the # brief documentation of file, namespace and class members alphabetically # by member name. If set to NO (the default) the members will appear in # declaration order. SORT_BRIEF_DOCS = NO # If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the # hierarchy of group names into alphabetical order. If set to NO (the default) # the group names will appear in their defined order. SORT_GROUP_NAMES = NO # If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be # sorted by fully-qualified names, including namespaces. If set to # NO (the default), the class list will be sorted only by class name, # not including the namespace part. # Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. # Note: This option applies only to the class list, not to the # alphabetical list. SORT_BY_SCOPE_NAME = NO # The GENERATE_TODOLIST tag can be used to enable (YES) or # disable (NO) the todo list. This list is created by putting \todo # commands in the documentation. GENERATE_TODOLIST = YES # The GENERATE_TESTLIST tag can be used to enable (YES) or # disable (NO) the test list. This list is created by putting \test # commands in the documentation. GENERATE_TESTLIST = YES # The GENERATE_BUGLIST tag can be used to enable (YES) or # disable (NO) the bug list. This list is created by putting \bug # commands in the documentation. GENERATE_BUGLIST = YES # The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or # disable (NO) the deprecated list. This list is created by putting # \deprecated commands in the documentation. GENERATE_DEPRECATEDLIST= YES # The ENABLED_SECTIONS tag can be used to enable conditional # documentation sections, marked by \if sectionname ... \endif. ENABLED_SECTIONS = # The MAX_INITIALIZER_LINES tag determines the maximum number of lines # the initial value of a variable or define consists of for it to appear in # the documentation. If the initializer consists of more lines than specified # here it will be hidden. Use a value of 0 to hide initializers completely. # The appearance of the initializer of individual variables and defines in the # documentation can be controlled using \showinitializer or \hideinitializer # command in the documentation regardless of this setting. MAX_INITIALIZER_LINES = 30 # Set the SHOW_USED_FILES tag to NO to disable the list of files generated # at the bottom of the documentation of classes and structs. If set to YES the # list will mention the files that were used to generate the documentation. SHOW_USED_FILES = YES # If the sources in your project are distributed over multiple directories # then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy # in the documentation. The default is NO. SHOW_DIRECTORIES = NO # Set the SHOW_FILES tag to NO to disable the generation of the Files page. # This will remove the Files entry from the Quick Index and from the # Folder Tree View (if specified). The default is YES. SHOW_FILES = YES # Set the SHOW_NAMESPACES tag to NO to disable the generation of the # Namespaces page. # This will remove the Namespaces entry from the Quick Index # and from the Folder Tree View (if specified). The default is YES. SHOW_NAMESPACES = YES # The FILE_VERSION_FILTER tag can be used to specify a program or script that # doxygen should invoke to get the current version for each file (typically from # the version control system). Doxygen will invoke the program by executing (via # popen()) the command , where is the value of # the FILE_VERSION_FILTER tag, and is the name of an input file # provided by doxygen. Whatever the program writes to standard output # is used as the file version. See the manual for examples. FILE_VERSION_FILTER = # The LAYOUT_FILE tag can be used to specify a layout file which will be parsed by # doxygen. The layout file controls the global structure of the generated output files # in an output format independent way. The create the layout file that represents # doxygen's defaults, run doxygen with the -l option. You can optionally specify a # file name after the option, if omitted DoxygenLayout.xml will be used as the name # of the layout file. LAYOUT_FILE = #--------------------------------------------------------------------------- # configuration options related to warning and progress messages #--------------------------------------------------------------------------- # The QUIET tag can be used to turn on/off the messages that are generated # by doxygen. Possible values are YES and NO. If left blank NO is used. QUIET = NO # The WARNINGS tag can be used to turn on/off the warning messages that are # generated by doxygen. Possible values are YES and NO. If left blank # NO is used. WARNINGS = YES # If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings # for undocumented members. If EXTRACT_ALL is set to YES then this flag will # automatically be disabled. WARN_IF_UNDOCUMENTED = YES # If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for # potential errors in the documentation, such as not documenting some # parameters in a documented function, or documenting parameters that # don't exist or using markup commands wrongly. WARN_IF_DOC_ERROR = YES # This WARN_NO_PARAMDOC option can be abled to get warnings for # functions that are documented, but have no documentation for their parameters # or return value. If set to NO (the default) doxygen will only warn about # wrong or incomplete parameter documentation, but not about the absence of # documentation. WARN_NO_PARAMDOC = NO # The WARN_FORMAT tag determines the format of the warning messages that # doxygen can produce. The string should contain the $file, $line, and $text # tags, which will be replaced by the file and line number from which the # warning originated and the warning text. Optionally the format may contain # $version, which will be replaced by the version of the file (if it could # be obtained via FILE_VERSION_FILTER) WARN_FORMAT = "$file:$line: $text" # The WARN_LOGFILE tag can be used to specify a file to which warning # and error messages should be written. If left blank the output is written # to stderr. WARN_LOGFILE = #--------------------------------------------------------------------------- # configuration options related to the input files #--------------------------------------------------------------------------- # The INPUT tag can be used to specify the files and/or directories that contain # documented source files. You may enter file names like "myfile.cpp" or # directories like "/usr/src/myproject". Separate the files or directories # with spaces. INPUT = src/Util src/Body src/Collision src/GuiBase src/MediaPlugin src/BodyPlugin src/ChoreographyPlugin # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is # also the default input encoding. Doxygen uses libiconv (or the iconv built # into libc) for the transcoding. See http://www.gnu.org/software/libiconv for # the list of possible encodings. INPUT_ENCODING = UTF-8 # If the value of the INPUT tag contains directories, you can use the # FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp # and *.h) to filter out the source-files in the directories. If left # blank the following patterns are tested: # *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx # *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90 FILE_PATTERNS = # The RECURSIVE tag can be used to turn specify whether or not subdirectories # should be searched for input files as well. Possible values are YES and NO. # If left blank NO is used. RECURSIVE = NO # The EXCLUDE tag can be used to specify files and/or directories that should # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. EXCLUDE = # The EXCLUDE_SYMLINKS tag can be used select whether or not files or # directories that are symbolic links (a Unix filesystem feature) are excluded # from the input. EXCLUDE_SYMLINKS = NO # If the value of the INPUT tag contains directories, you can use the # EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude # certain files from those directories. Note that the wildcards are matched # against the file with absolute path, so to exclude all test directories # for example use the pattern */test/* EXCLUDE_PATTERNS = */test/* */old/* */.svn/* # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names # (namespaces, classes, functions, etc.) that should be excluded from the # output. The symbol name can be a fully qualified name, a word, or if the # wildcard * is used, a substring. Examples: ANamespace, AClass, # AClass::ANamespace, ANamespace::*Test EXCLUDE_SYMBOLS = # The EXAMPLE_PATH tag can be used to specify one or more files or # directories that contain example code fragments that are included (see # the \include command). EXAMPLE_PATH = # If the value of the EXAMPLE_PATH tag contains directories, you can use the # EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp # and *.h) to filter out the source-files in the directories. If left # blank all files are included. EXAMPLE_PATTERNS = # If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be # searched for input files to be used with the \include or \dontinclude # commands irrespective of the value of the RECURSIVE tag. # Possible values are YES and NO. If left blank NO is used. EXAMPLE_RECURSIVE = NO # The IMAGE_PATH tag can be used to specify one or more files or # directories that contain image that are included in the documentation (see # the \image command). IMAGE_PATH = # The INPUT_FILTER tag can be used to specify a program that doxygen should # invoke to filter for each input file. Doxygen will invoke the filter program # by executing (via popen()) the command , where # is the value of the INPUT_FILTER tag, and is the name of an # input file. Doxygen will then use the output that the filter program writes # to standard output. # If FILTER_PATTERNS is specified, this tag will be # ignored. INPUT_FILTER = # The FILTER_PATTERNS tag can be used to specify filters on a per file pattern # basis. # Doxygen will compare the file name with each pattern and apply the # filter if there is a match. # The filters are a list of the form: # pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further # info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER # is applied to all files. FILTER_PATTERNS = # If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using # INPUT_FILTER) will be used to filter the input files when producing source # files to browse (i.e. when SOURCE_BROWSER is set to YES). FILTER_SOURCE_FILES = NO #--------------------------------------------------------------------------- # configuration options related to source browsing #--------------------------------------------------------------------------- # If the SOURCE_BROWSER tag is set to YES then a list of source files will # be generated. Documented entities will be cross-referenced with these sources. # Note: To get rid of all source code in the generated output, make sure also # VERBATIM_HEADERS is set to NO. SOURCE_BROWSER = NO # Setting the INLINE_SOURCES tag to YES will include the body # of functions and classes directly in the documentation. INLINE_SOURCES = NO # Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct # doxygen to hide any special comment blocks from generated source code # fragments. Normal C and C++ comments will always remain visible. STRIP_CODE_COMMENTS = YES # If the REFERENCED_BY_RELATION tag is set to YES # then for each documented function all documented # functions referencing it will be listed. REFERENCED_BY_RELATION = NO # If the REFERENCES_RELATION tag is set to YES # then for each documented function all documented entities # called/used by that function will be listed. REFERENCES_RELATION = NO # If the REFERENCES_LINK_SOURCE tag is set to YES (the default) # and SOURCE_BROWSER tag is set to YES, then the hyperlinks from # functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will # link to the source code. # Otherwise they will link to the documentation. REFERENCES_LINK_SOURCE = YES # If the USE_HTAGS tag is set to YES then the references to source code # will point to the HTML generated by the htags(1) tool instead of doxygen # built-in source browser. The htags tool is part of GNU's global source # tagging system (see http://www.gnu.org/software/global/global.html). You # will need version 4.8.6 or higher. USE_HTAGS = NO # If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen # will generate a verbatim copy of the header file for each class for # which an include is specified. Set to NO to disable this. VERBATIM_HEADERS = YES #--------------------------------------------------------------------------- # configuration options related to the alphabetical class index #--------------------------------------------------------------------------- # If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index # of all compounds will be generated. Enable this if the project # contains a lot of classes, structs, unions or interfaces. ALPHABETICAL_INDEX = YES # If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then # the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns # in which this list will be split (can be a number in the range [1..20]) COLS_IN_ALPHA_INDEX = 5 # In case all classes in a project start with a common prefix, all # classes will be put under the same header in the alphabetical index. # The IGNORE_PREFIX tag can be used to specify one or more prefixes that # should be ignored while generating the index headers. IGNORE_PREFIX = #--------------------------------------------------------------------------- # configuration options related to the HTML output #--------------------------------------------------------------------------- # If the GENERATE_HTML tag is set to YES (the default) Doxygen will # generate HTML output. GENERATE_HTML = YES # The HTML_OUTPUT tag is used to specify where the HTML docs will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be # put in front of it. If left blank `html' will be used as the default path. HTML_OUTPUT = html # The HTML_FILE_EXTENSION tag can be used to specify the file extension for # each generated HTML page (for example: .htm,.php,.asp). If it is left blank # doxygen will generate files with .html extension. HTML_FILE_EXTENSION = .html # The HTML_HEADER tag can be used to specify a personal HTML header for # each generated HTML page. If it is left blank doxygen will generate a # standard header. HTML_HEADER = # The HTML_FOOTER tag can be used to specify a personal HTML footer for # each generated HTML page. If it is left blank doxygen will generate a # standard footer. HTML_FOOTER = # The HTML_STYLESHEET tag can be used to specify a user-defined cascading # style sheet that is used by each HTML page. It can be used to # fine-tune the look of the HTML output. If the tag is left blank doxygen # will generate a default style sheet. Note that doxygen will try to copy # the style sheet file to the HTML output directory, so don't put your own # stylesheet in the HTML output directory as well, or it will be erased! HTML_STYLESHEET = # If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, # files or namespaces will be aligned in HTML using tables. If set to # NO a bullet list will be used. HTML_ALIGN_MEMBERS = YES # If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML # documentation will contain sections that can be hidden and shown after the # page has loaded. For this to work a browser that supports # JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox # Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). HTML_DYNAMIC_SECTIONS = NO # If the GENERATE_DOCSET tag is set to YES, additional index files # will be generated that can be used as input for Apple's Xcode 3 # integrated development environment, introduced with OSX 10.5 (Leopard). # To create a documentation set, doxygen will generate a Makefile in the # HTML output directory. Running make will produce the docset in that # directory and running "make install" will install the docset in # ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find # it at startup. # See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html for more information. GENERATE_DOCSET = NO # When GENERATE_DOCSET tag is set to YES, this tag determines the name of the # feed. A documentation feed provides an umbrella under which multiple # documentation sets from a single provider (such as a company or product suite) # can be grouped. DOCSET_FEEDNAME = "Doxygen generated docs" # When GENERATE_DOCSET tag is set to YES, this tag specifies a string that # should uniquely identify the documentation set bundle. This should be a # reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen # will append .docset to the name. DOCSET_BUNDLE_ID = org.doxygen.Project # If the GENERATE_HTMLHELP tag is set to YES, additional index files # will be generated that can be used as input for tools like the # Microsoft HTML help workshop to generate a compiled HTML help file (.chm) # of the generated HTML documentation. GENERATE_HTMLHELP = NO # If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can # be used to specify the file name of the resulting .chm file. You # can add a path in front of the file if the result should not be # written to the html output directory. CHM_FILE = # If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can # be used to specify the location (absolute path including file name) of # the HTML help compiler (hhc.exe). If non-empty doxygen will try to run # the HTML help compiler on the generated index.hhp. HHC_LOCATION = # If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag # controls if a separate .chi index file is generated (YES) or that # it should be included in the master .chm file (NO). GENERATE_CHI = NO # If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING # is used to encode HtmlHelp index (hhk), content (hhc) and project file # content. CHM_INDEX_ENCODING = # If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag # controls whether a binary table of contents is generated (YES) or a # normal table of contents (NO) in the .chm file. BINARY_TOC = NO # The TOC_EXPAND flag can be set to YES to add extra items for group members # to the contents of the HTML help documentation and to the tree view. TOC_EXPAND = NO # If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and QHP_VIRTUAL_FOLDER # are set, an additional index file will be generated that can be used as input for # Qt's qhelpgenerator to generate a Qt Compressed Help (.qch) of the generated # HTML documentation. GENERATE_QHP = NO # If the QHG_LOCATION tag is specified, the QCH_FILE tag can # be used to specify the file name of the resulting .qch file. # The path specified is relative to the HTML output folder. QCH_FILE = # The QHP_NAMESPACE tag specifies the namespace to use when generating # Qt Help Project output. For more information please see # http://doc.trolltech.com/qthelpproject.html#namespace QHP_NAMESPACE = # The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating # Qt Help Project output. For more information please see # http://doc.trolltech.com/qthelpproject.html#virtual-folders QHP_VIRTUAL_FOLDER = doc # If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to add. # For more information please see # http://doc.trolltech.com/qthelpproject.html#custom-filters QHP_CUST_FILTER_NAME = # The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the custom filter to add.For more information please see # Qt Help Project / Custom Filters. QHP_CUST_FILTER_ATTRS = # The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this project's # filter section matches. # Qt Help Project / Filter Attributes. QHP_SECT_FILTER_ATTRS = # If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can # be used to specify the location of Qt's qhelpgenerator. # If non-empty doxygen will try to run qhelpgenerator on the generated # .qhp file. QHG_LOCATION = # The DISABLE_INDEX tag can be used to turn on/off the condensed index at # top of each HTML page. The value NO (the default) enables the index and # the value YES disables it. DISABLE_INDEX = NO # This tag can be used to set the number of enum values (range [1..20]) # that doxygen will group on one line in the generated HTML documentation. ENUM_VALUES_PER_LINE = 4 # The GENERATE_TREEVIEW tag is used to specify whether a tree-like index # structure should be generated to display hierarchical information. # If the tag value is set to FRAME, a side panel will be generated # containing a tree-like index structure (just like the one that # is generated for HTML Help). For this to work a browser that supports # JavaScript, DHTML, CSS and frames is required (for instance Mozilla 1.0+, # Netscape 6.0+, Internet explorer 5.0+, or Konqueror). Windows users are # probably better off using the HTML help feature. Other possible values # for this tag are: HIERARCHIES, which will generate the Groups, Directories, # and Class Hierarchy pages using a tree view instead of an ordered list; # ALL, which combines the behavior of FRAME and HIERARCHIES; and NONE, which # disables this behavior completely. For backwards compatibility with previous # releases of Doxygen, the values YES and NO are equivalent to FRAME and NONE # respectively. GENERATE_TREEVIEW = NONE # If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be # used to set the initial width (in pixels) of the frame in which the tree # is shown. TREEVIEW_WIDTH = 250 # Use this tag to change the font size of Latex formulas included # as images in the HTML documentation. The default is 10. Note that # when you change the font size after a successful doxygen run you need # to manually remove any form_*.png images from the HTML output directory # to force them to be regenerated. FORMULA_FONTSIZE = 10 #--------------------------------------------------------------------------- # configuration options related to the LaTeX output #--------------------------------------------------------------------------- # If the GENERATE_LATEX tag is set to YES (the default) Doxygen will # generate Latex output. GENERATE_LATEX = No # The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be # put in front of it. If left blank `latex' will be used as the default path. LATEX_OUTPUT = latex # The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be # invoked. If left blank `latex' will be used as the default command name. LATEX_CMD_NAME = latex # The MAKEINDEX_CMD_NAME tag can be used to specify the command name to # generate index for LaTeX. If left blank `makeindex' will be used as the # default command name. MAKEINDEX_CMD_NAME = makeindex # If the COMPACT_LATEX tag is set to YES Doxygen generates more compact # LaTeX documents. This may be useful for small projects and may help to # save some trees in general. COMPACT_LATEX = NO # The PAPER_TYPE tag can be used to set the paper type that is used # by the printer. Possible values are: a4, a4wide, letter, legal and # executive. If left blank a4wide will be used. PAPER_TYPE = a4wide # The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX # packages that should be included in the LaTeX output. EXTRA_PACKAGES = # The LATEX_HEADER tag can be used to specify a personal LaTeX header for # the generated latex document. The header should contain everything until # the first chapter. If it is left blank doxygen will generate a # standard header. Notice: only use this tag if you know what you are doing! LATEX_HEADER = # If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated # is prepared for conversion to pdf (using ps2pdf). The pdf file will # contain links (just like the HTML output) instead of page references # This makes the output suitable for online browsing using a pdf viewer. PDF_HYPERLINKS = YES # If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of # plain latex in the generated Makefile. Set this option to YES to get a # higher quality PDF documentation. USE_PDFLATEX = YES # If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. # command to the generated LaTeX files. This will instruct LaTeX to keep # running if errors occur, instead of asking the user for help. # This option is also used when generating formulas in HTML. LATEX_BATCHMODE = NO # If LATEX_HIDE_INDICES is set to YES then doxygen will not # include the index chapters (such as File Index, Compound Index, etc.) # in the output. LATEX_HIDE_INDICES = NO #--------------------------------------------------------------------------- # configuration options related to the RTF output #--------------------------------------------------------------------------- # If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output # The RTF output is optimized for Word 97 and may not look very pretty with # other RTF readers or editors. GENERATE_RTF = NO # The RTF_OUTPUT tag is used to specify where the RTF docs will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be # put in front of it. If left blank `rtf' will be used as the default path. RTF_OUTPUT = rtf # If the COMPACT_RTF tag is set to YES Doxygen generates more compact # RTF documents. This may be useful for small projects and may help to # save some trees in general. COMPACT_RTF = NO # If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated # will contain hyperlink fields. The RTF file will # contain links (just like the HTML output) instead of page references. # This makes the output suitable for online browsing using WORD or other # programs which support those fields. # Note: wordpad (write) and others do not support links. RTF_HYPERLINKS = NO # Load stylesheet definitions from file. Syntax is similar to doxygen's # config file, i.e. a series of assignments. You only have to provide # replacements, missing definitions are set to their default value. RTF_STYLESHEET_FILE = # Set optional variables used in the generation of an rtf document. # Syntax is similar to doxygen's config file. RTF_EXTENSIONS_FILE = #--------------------------------------------------------------------------- # configuration options related to the man page output #--------------------------------------------------------------------------- # If the GENERATE_MAN tag is set to YES (the default) Doxygen will # generate man pages GENERATE_MAN = NO # The MAN_OUTPUT tag is used to specify where the man pages will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be # put in front of it. If left blank `man' will be used as the default path. MAN_OUTPUT = man # The MAN_EXTENSION tag determines the extension that is added to # the generated man pages (default is the subroutine's section .3) MAN_EXTENSION = .3 # If the MAN_LINKS tag is set to YES and Doxygen generates man output, # then it will generate one additional man file for each entity # documented in the real man page(s). These additional files # only source the real man page, but without them the man command # would be unable to find the correct page. The default is NO. MAN_LINKS = NO #--------------------------------------------------------------------------- # configuration options related to the XML output #--------------------------------------------------------------------------- # If the GENERATE_XML tag is set to YES Doxygen will # generate an XML file that captures the structure of # the code including all documentation. GENERATE_XML = NO # The XML_OUTPUT tag is used to specify where the XML pages will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be # put in front of it. If left blank `xml' will be used as the default path. XML_OUTPUT = xml # The XML_SCHEMA tag can be used to specify an XML schema, # which can be used by a validating XML parser to check the # syntax of the XML files. XML_SCHEMA = # The XML_DTD tag can be used to specify an XML DTD, # which can be used by a validating XML parser to check the # syntax of the XML files. XML_DTD = # If the XML_PROGRAMLISTING tag is set to YES Doxygen will # dump the program listings (including syntax highlighting # and cross-referencing information) to the XML output. Note that # enabling this will significantly increase the size of the XML output. XML_PROGRAMLISTING = YES #--------------------------------------------------------------------------- # configuration options for the AutoGen Definitions output #--------------------------------------------------------------------------- # If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will # generate an AutoGen Definitions (see autogen.sf.net) file # that captures the structure of the code including all # documentation. Note that this feature is still experimental # and incomplete at the moment. GENERATE_AUTOGEN_DEF = NO #--------------------------------------------------------------------------- # configuration options related to the Perl module output #--------------------------------------------------------------------------- # If the GENERATE_PERLMOD tag is set to YES Doxygen will # generate a Perl module file that captures the structure of # the code including all documentation. Note that this # feature is still experimental and incomplete at the # moment. GENERATE_PERLMOD = NO # If the PERLMOD_LATEX tag is set to YES Doxygen will generate # the necessary Makefile rules, Perl scripts and LaTeX code to be able # to generate PDF and DVI output from the Perl module output. PERLMOD_LATEX = NO # If the PERLMOD_PRETTY tag is set to YES the Perl module output will be # nicely formatted so it can be parsed by a human reader. # This is useful # if you want to understand what is going on. # On the other hand, if this # tag is set to NO the size of the Perl module output will be much smaller # and Perl will parse it just the same. PERLMOD_PRETTY = YES # The names of the make variables in the generated doxyrules.make file # are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. # This is useful so different doxyrules.make files included by the same # Makefile don't overwrite each other's variables. PERLMOD_MAKEVAR_PREFIX = #--------------------------------------------------------------------------- # Configuration options related to the preprocessor #--------------------------------------------------------------------------- # If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will # evaluate all C-preprocessor directives found in the sources and include # files. ENABLE_PREPROCESSING = YES # If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro # names in the source code. If set to NO (the default) only conditional # compilation will be performed. Macro expansion can be done in a controlled # way by setting EXPAND_ONLY_PREDEF to YES. MACRO_EXPANSION = NO # If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES # then the macro expansion is limited to the macros specified with the # PREDEFINED and EXPAND_AS_DEFINED tags. EXPAND_ONLY_PREDEF = NO # If the SEARCH_INCLUDES tag is set to YES (the default) the includes files # in the INCLUDE_PATH (see below) will be search if a #include is found. SEARCH_INCLUDES = YES # The INCLUDE_PATH tag can be used to specify one or more directories that # contain include files that are not input files but should be processed by # the preprocessor. INCLUDE_PATH = # You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard # patterns (like *.h and *.hpp) to filter out the header-files in the # directories. If left blank, the patterns specified with FILE_PATTERNS will # be used. INCLUDE_FILE_PATTERNS = # The PREDEFINED tag can be used to specify one or more macro names that # are defined before the preprocessor is started (similar to the -D option of # gcc). The argument of the tag is a list of macros of the form: name # or name=definition (no spaces). If the definition and the = are # omitted =1 is assumed. To prevent a macro definition from being # undefined via #undef or recursively expanded use the := operator # instead of the = operator. PREDEFINED = # If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then # this tag can be used to specify a list of macro names that should be expanded. # The macro definition that is found in the sources will be used. # Use the PREDEFINED tag if you want to use a different macro definition. EXPAND_AS_DEFINED = # If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then # doxygen's preprocessor will remove all function-like macros that are alone # on a line, have an all uppercase name, and do not end with a semicolon. Such # function macros are typically used for boiler-plate code, and will confuse # the parser if not removed. SKIP_FUNCTION_MACROS = YES #--------------------------------------------------------------------------- # Configuration::additions related to external references #--------------------------------------------------------------------------- # The TAGFILES option can be used to specify one or more tagfiles. # Optionally an initial location of the external documentation # can be added for each tagfile. The format of a tag file without # this location is as follows: # # TAGFILES = file1 file2 ... # Adding location for the tag files is done as follows: # # TAGFILES = file1=loc1 "file2 = loc2" ... # where "loc1" and "loc2" can be relative or absolute paths or # URLs. If a location is present for each tag, the installdox tool # does not have to be run to correct the links. # Note that each tag file must have a unique name # (where the name does NOT include the path) # If a tag file is not located in the directory in which doxygen # is run, you must also specify the path to the tagfile here. TAGFILES = # When a file name is specified after GENERATE_TAGFILE, doxygen will create # a tag file that is based on the input files it reads. GENERATE_TAGFILE = # If the ALLEXTERNALS tag is set to YES all external classes will be listed # in the class index. If set to NO only the inherited external classes # will be listed. ALLEXTERNALS = NO # If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed # in the modules index. If set to NO, only the current project's groups will # be listed. EXTERNAL_GROUPS = YES # The PERL_PATH should be the absolute path and name of the perl script # interpreter (i.e. the result of `which perl'). PERL_PATH = /usr/bin/perl #--------------------------------------------------------------------------- # Configuration options related to the dot tool #--------------------------------------------------------------------------- # If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will # generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base # or super classes. Setting the tag to NO turns the diagrams off. Note that # this option is superseded by the HAVE_DOT option below. This is only a # fallback. It is recommended to install and use dot, since it yields more # powerful graphs. CLASS_DIAGRAMS = YES # You can define message sequence charts within doxygen comments using the \msc # command. Doxygen will then run the mscgen tool (see # http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the # documentation. The MSCGEN_PATH tag allows you to specify the directory where # the mscgen tool resides. If left empty the tool is assumed to be found in the # default search path. MSCGEN_PATH = # If set to YES, the inheritance and collaboration graphs will hide # inheritance and usage relations if the target is undocumented # or is not a class. HIDE_UNDOC_RELATIONS = YES # If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is # available from the path. This tool is part of Graphviz, a graph visualization # toolkit from AT&T and Lucent Bell Labs. The other options in this section # have no effect if this option is set to NO (the default) HAVE_DOT = NO # By default doxygen will write a font called FreeSans.ttf to the output # directory and reference it in all dot files that doxygen generates. This # font does not include all possible unicode characters however, so when you need # these (or just want a differently looking font) you can specify the font name # using DOT_FONTNAME. You need need to make sure dot is able to find the font, # which can be done by putting it in a standard location or by setting the # DOTFONTPATH environment variable or by setting DOT_FONTPATH to the directory # containing the font. DOT_FONTNAME = FreeSans # The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. # The default size is 10pt. DOT_FONTSIZE = 10 # By default doxygen will tell dot to use the output directory to look for the # FreeSans.ttf font (which doxygen will put there itself). If you specify a # different font using DOT_FONTNAME you can set the path where dot # can find it using this tag. DOT_FONTPATH = # If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen # will generate a graph for each documented class showing the direct and # indirect inheritance relations. Setting this tag to YES will force the # the CLASS_DIAGRAMS tag to NO. CLASS_GRAPH = YES # If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen # will generate a graph for each documented class showing the direct and # indirect implementation dependencies (inheritance, containment, and # class references variables) of the class with other documented classes. #COLLABORATION_GRAPH = YES COLLABORATION_GRAPH = YES # If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen # will generate a graph for groups, showing the direct groups dependencies GROUP_GRAPHS = YES # If the UML_LOOK tag is set to YES doxygen will generate inheritance and # collaboration diagrams in a style similar to the OMG's Unified Modeling # Language. UML_LOOK = NO # If set to YES, the inheritance and collaboration graphs will show the # relations between templates and their instances. #TEMPLATE_RELATIONS = NO TEMPLATE_RELATIONS = YES # If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT # tags are set to YES then doxygen will generate a graph for each documented # file showing the direct and indirect include dependencies of the file with # other documented files. INCLUDE_GRAPH = YES # If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and # HAVE_DOT tags are set to YES then doxygen will generate a graph for each # documented header file showing the documented files that directly or # indirectly include this file. INCLUDED_BY_GRAPH = YES # If the CALL_GRAPH and HAVE_DOT options are set to YES then # doxygen will generate a call dependency graph for every global function # or class method. Note that enabling this option will significantly increase # the time of a run. So in most cases it will be better to enable call graphs # for selected functions only using the \callgraph command. CALL_GRAPH = NO # If the CALLER_GRAPH and HAVE_DOT tags are set to YES then # doxygen will generate a caller dependency graph for every global function # or class method. Note that enabling this option will significantly increase # the time of a run. So in most cases it will be better to enable caller # graphs for selected functions only using the \callergraph command. CALLER_GRAPH = NO # If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen # will graphical hierarchy of all classes instead of a textual one. GRAPHICAL_HIERARCHY = YES # If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES # then doxygen will show the dependencies a directory has on other directories # in a graphical way. The dependency relations are determined by the #include # relations between the files in the directories. DIRECTORY_GRAPH = YES # The DOT_IMAGE_FORMAT tag can be used to set the image format of the images # generated by dot. Possible values are png, jpg, or gif # If left blank png will be used. DOT_IMAGE_FORMAT = png # The tag DOT_PATH can be used to specify the path where the dot tool can be # found. If left blank, it is assumed the dot tool can be found in the path. DOT_PATH = # The DOTFILE_DIRS tag can be used to specify one or more directories that # contain dot files that are included in the documentation (see the # \dotfile command). DOTFILE_DIRS = # The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of # nodes that will be shown in the graph. If the number of nodes in a graph # becomes larger than this value, doxygen will truncate the graph, which is # visualized by representing a node as a red box. Note that doxygen if the # number of direct children of the root node in a graph is already larger than # DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note # that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. DOT_GRAPH_MAX_NODES = 50 # The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the # graphs generated by dot. A depth value of 3 means that only nodes reachable # from the root by following a path via at most 3 edges will be shown. Nodes # that lay further from the root node will be omitted. Note that setting this # option to 1 or 2 may greatly reduce the computation time needed for large # code bases. Also note that the size of a graph can be further restricted by # DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. MAX_DOT_GRAPH_DEPTH = 0 # Set the DOT_TRANSPARENT tag to YES to generate images with a transparent # background. This is disabled by default, because dot on Windows does not # seem to support this out of the box. Warning: Depending on the platform used, # enabling this option may lead to badly anti-aliased labels on the edges of # a graph (i.e. they become hard to read). DOT_TRANSPARENT = NO # Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output # files in one run (i.e. multiple -o and -T options on the command line). This # makes dot run faster, but since only newer versions of dot (>1.8.10) # support this, this feature is disabled by default. DOT_MULTI_TARGETS = YES # If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will # generate a legend page explaining the meaning of the various boxes and # arrows in the dot generated graphs. GENERATE_LEGEND = YES # If the DOT_CLEANUP tag is set to YES (the default) Doxygen will # remove the intermediate dot files that are used to generate # the various graphs. DOT_CLEANUP = YES #--------------------------------------------------------------------------- # Options related to the search engine #--------------------------------------------------------------------------- # The SEARCHENGINE tag specifies whether or not a search engine should be # used. If set to NO the values of all tags below this one will be ignored. SEARCHENGINE = NO choreonoid-1.1.0+dfsg/INSTALL000066400000000000000000000066141207742442300156260ustar00rootroot00000000000000 How to compile and install Choreonoid ===================================== For Ubuntu Linux ================ For the Ubuntu Linux version 10.04 - 11.10, Choreonoid can be compiled according to the following instructions. 1. Installing packages needed for compiling Choreonoid By invoking "install-requisites-ubuntu.sh" in the "misc/script" directory, the tools and libraries required for compiling Choreonoid are installed. 2. Generating Makefiles with CMake Choreonoid uses CMake (http://www.cmake.org/) as a standard build system. The building procedure of Choreonoid is described by CMake scripts, and Makefiles can be generated by using CMake commands. For example, type the following command in the top source directory: $ cmake . Then Makefiles for compiling Choreonoid are generated. You can also specify various options for the compile. Please see the CMake documentation to do that. 3. Compile Please type the make command: $ make If your PC has multi-core processors, parallel compile with -j option is useful to shorten the whole compile time. For example, if you want to simultaneously invoke eight processes for the compile, type the following command: $ make -j 8 After the compile succeeds, execution file "choreonoid" is generated in the "bin" directory. You can invoke Choreonoid by typing the following command: $ bin/choreonoid 4. Installation As is described above, Choreonoid binaries generated in the original source tree can be directly used without installation. However, if you want to install them into the system directories, it can be done by the following command: $ make install In default, program and data files are installed into the "/usr/local" directory. It can be changed by setting the "CMAKE_INSTALL_PREFIX" variable of CMake. For Windows =========== 1. Installing compile tools Currently we have only tested the compile using Visual C++ 2008 and 2010. So please install either of them in advance. As well as Linux, CMake (http://www.cmake.org/) is used for describing the compile procedure and project files of Visual Studio are generated with it. So please install CMake in advance. 2. Installing requisite libraries Please manually install the following libraries: * Boost C++ Libraries Since choreonoid requires some binary files of the boost libraries, you should make the boost binaries or get some binary packages. The site of Boost Pro (http://www.boostpro.com) provides binary packages of the boost, and downloading a package and installing it is a good solution. * Qt You can get it from http://qt.nokia.com/. * OpenSceneGraph You can get it from http://www.openscenegraph.org/. 3. Generating project files with CMake Please generate Visual Studio project files by using CMake. Using "cmake-gui.exe" is recommended and you should manually specify the paths of the above libraries. 4. Building Choreonoid using Visual Studio By loading a solution file (.sln) generated by CMake, the projects for building Choreonoid appear on the Visual Studio IDE. By building the "ALL_BUILD" project, all components are built. Unlike the compile on Linux, the binary files generated under the original source (building) directory cannot be directly executed from there, and the installation is required. By building the "INSTALL" project on the Visual Studio IDE, the program and data of Choreonoid are installed into the directory specified with the "CMAKE_INSTALL_PREFIX" variable. choreonoid-1.1.0+dfsg/LICENSE000066400000000000000000000030321207742442300155710ustar00rootroot00000000000000This program has been developed by Shin'ichiro Nakaoka and Choreonoid Development Team, AIST. This program is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This package is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. The source and some binary packages of this program also include the following third-party libraries: * Eigen (http://eigen.tuxfamily.org/) * IJG JPEG Library (http://www.ijg.org/) * libpng (http://www.libpng.org/pub/png/libpng.html) * LibYAML (http://pyyaml.org/wiki/LibYAML) * OPCODE (http://www.codercorner.com/Opcode.htm) * zlib (http://zlib.net/) These libraries are used and redistributed under the terms of their licenses. Please see the corresponding directories in the source package or their official web sites to see the details of their licenses. This program also depends on a lot of other external libraries, which are linked into this program during the execution. Some binary packages of this program may include the binaries of the following libraries: * Qt (http://qt.nokia.com/) * OpenSceneGraph (http://www.openscenegraph.org) * Boost C++ Libraries (http://www.boost.org/) Please see their official web sites to see the details of their licenses. choreonoid-1.1.0+dfsg/NEWS000066400000000000000000000012651207742442300152710ustar00rootroot00000000000000 Overview of Changes in Choreonoid 1.1.0 ======================================= * Supported internationalization - Messages can be translated into other languages - Added Japanese translations - International characters (any characters defined in Unicode) can be used in data and filenames in the Linux distributions based on UTF-8 - Local character encoding (such as shift-jis) can be used in data and filenames in Windows - Added some scripts for handling gettext files * New graphical tool bar icons * New application icon * Fixed a compile error in Visual C++ 2010 * Some bug fixes Overview of Choreonoid 1.0.0 ============================ * The initial release version choreonoid-1.1.0+dfsg/cleanup-script.sh000077500000000000000000000006031207742442300200550ustar00rootroot00000000000000#!/bin/sh # WARNING: to be run in the package top source directory. # Remove 3rd-party dependencies as they are not needed. rm -rf thirdparty # Remove proprietary plug-ins we cannot package anyway. rm -rf proprietary # Comment the add_subdirectory corresponding to the 3rd party directory. sed -i -e 's|add_subdirectory(thirdparty)|#add_subdirectory(thirdparty)|g' \ CMakeLists.txt choreonoid-1.1.0+dfsg/extplugin/000077500000000000000000000000001207742442300166055ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/extplugin/CMakeLists.txt000066400000000000000000000006141207742442300213460ustar00rootroot00000000000000# @author Shin'ichiro Nakaoka if(ENABLE_INSTALL_RPATH) unset(CMAKE_INSTALL_RPATH) endif() if(UNIX) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CNOID_PLUGIN_SUBDIR}) endif() file(GLOB plugins "*Plugin" "cnoid/*Plugin" "sample/*Plugin") foreach(subdirectory ${plugins}) if(EXISTS ${subdirectory}/CMakeLists.txt) add_subdirectory(${subdirectory}) endif() endforeach() choreonoid-1.1.0+dfsg/extplugin/sample/000077500000000000000000000000001207742442300200665ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/extplugin/sample/GLGearsPlugin/000077500000000000000000000000001207742442300225315ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/extplugin/sample/GLGearsPlugin/CMakeLists.txt000066400000000000000000000006511207742442300252730ustar00rootroot00000000000000 option(BUILD_GL_GEARS_SAMPLE "Building a GL Gears sample plugin" OFF) if(BUILD_GL_GEARS_SAMPLE) set(srcdir ${PROJECT_SOURCE_DIR}/share/sampleplugins/GLGearsPlugin) set(sources ${srcdir}/GLGearsPlugin.cpp ${srcdir}/GLGearsView.cpp ) set(target CnoidGLGearsPlugin) add_library(${target} SHARED ${sources}) target_link_libraries(${target} CnoidBase) apply_common_setting_for_plugin(${target}) endif() choreonoid-1.1.0+dfsg/extplugin/sample/HelloWorldPlugin/000077500000000000000000000000001207742442300233205ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/extplugin/sample/HelloWorldPlugin/CMakeLists.txt000066400000000000000000000005711207742442300260630ustar00rootroot00000000000000 option(BUILD_HELLO_WORLD_SAMPLE "Building a Hello World sample plugin" OFF) if(BUILD_HELLO_WORLD_SAMPLE) set(target CnoidHelloWorldPlugin) set(srcdir ${PROJECT_SOURCE_DIR}/share/sampleplugins/HelloWorldPlugin) add_library(${target} SHARED ${srcdir}/HelloWorldPlugin.cpp) target_link_libraries(${target} CnoidBase) apply_common_setting_for_plugin(${target}) endif() choreonoid-1.1.0+dfsg/extplugin/sample/Sample1Plugin/000077500000000000000000000000001207742442300225475ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/extplugin/sample/Sample1Plugin/CMakeLists.txt000066400000000000000000000005641207742442300253140ustar00rootroot00000000000000 option(BUILD_SAMPLE1_SAMPLE "Building a sample plugin \"Sample1Plugin\"" OFF) if(BUILD_SAMPLE1_SAMPLE) set(target CnoidSample1Plugin) set(srcdir ${PROJECT_SOURCE_DIR}/share/sampleplugins/Sample1Plugin) add_library(${target} SHARED ${srcdir}/Sample1Plugin.cpp) target_link_libraries(${target} CnoidBodyPlugin) apply_common_setting_for_plugin(${target}) endif() choreonoid-1.1.0+dfsg/include/000077500000000000000000000000001207742442300162115ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/include/CMakeLists.txt000066400000000000000000000003431207742442300207510ustar00rootroot00000000000000 if(INSTALL_SDK) file(GLOB public_headers "cnoid/*") foreach(file ${public_headers}) if(NOT IS_DIRECTORY ${file}) install(FILES ${file} DESTINATION ${CNOID_HEADER_SUBDIR}/cnoid) endif() endforeach() endif() choreonoid-1.1.0+dfsg/include/cnoid/000077500000000000000000000000001207742442300173055ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/include/cnoid/Action000066400000000000000000000000351207742442300204430ustar00rootroot00000000000000#include "src/Base/Action.h" choreonoid-1.1.0+dfsg/include/cnoid/App000066400000000000000000000000321207742442300177430ustar00rootroot00000000000000#include "src/Base/App.h" choreonoid-1.1.0+dfsg/include/cnoid/AppConfig000066400000000000000000000000401207742442300210700ustar00rootroot00000000000000#include "src/Base/AppConfig.h" choreonoid-1.1.0+dfsg/include/cnoid/Archive000066400000000000000000000000361207742442300206100ustar00rootroot00000000000000#include "src/Base/Archive.h" choreonoid-1.1.0+dfsg/include/cnoid/Body000066400000000000000000000000331207742442300201210ustar00rootroot00000000000000#include "src/Body/Body.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyBar000066400000000000000000000000441207742442300205500ustar00rootroot00000000000000#include "src/BodyPlugin/BodyBar.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyClasses000066400000000000000000000002631207742442300214440ustar00rootroot00000000000000#include #include #include #include #include #include #include choreonoid-1.1.0+dfsg/include/cnoid/BodyCustomizerInterface000066400000000000000000000000561207742442300240340ustar00rootroot00000000000000#include "src/Body/BodyCustomizerInterface.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyItem000066400000000000000000000000451207742442300207430ustar00rootroot00000000000000#include "src/BodyPlugin/BodyItem.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyItemUpdater000066400000000000000000000000541207742442300222700ustar00rootroot00000000000000#include "src/BodyPlugin/BodyItemUpdater.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyLoader000066400000000000000000000000411207742442300212470ustar00rootroot00000000000000#include "src/Body/BodyLoader.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyMotion000066400000000000000000000000411207742442300213060ustar00rootroot00000000000000#include "src/Body/BodyMotion.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyMotionGenerationBar000066400000000000000000000000751207742442300237560ustar00rootroot00000000000000#include "../../src/PoseSeqPlugin/BodyMotionGenerationBar.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyMotionItem000066400000000000000000000000531207742442300221300ustar00rootroot00000000000000#include "src/BodyPlugin/BodyMotionItem.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyMotionPoseProvider000066400000000000000000000000551207742442300236550ustar00rootroot00000000000000#include "src/Body/BodyMotionPoseProvider.h" choreonoid-1.1.0+dfsg/include/cnoid/BodyMotionUtil000066400000000000000000000000451207742442300221500ustar00rootroot00000000000000#include "src/Body/BodyMotionUtil.h" choreonoid-1.1.0+dfsg/include/cnoid/Button000066400000000000000000000000351207742442300205010ustar00rootroot00000000000000#include "src/Base/Button.h" choreonoid-1.1.0+dfsg/include/cnoid/ButtonGroup000066400000000000000000000000421207742442300215140ustar00rootroot00000000000000#include "src/Base/ButtonGroup.h" choreonoid-1.1.0+dfsg/include/cnoid/ColdetLinkPair000066400000000000000000000000451207742442300220730ustar00rootroot00000000000000#include "src/Body/ColdetLinkPair.h" choreonoid-1.1.0+dfsg/include/cnoid/ColdetModel000066400000000000000000000000471207742442300214240ustar00rootroot00000000000000#include "src/Collision/ColdetModel.h" choreonoid-1.1.0+dfsg/include/cnoid/ColdetModelPair000066400000000000000000000000531207742442300222350ustar00rootroot00000000000000#include "src/Collision/ColdetModelPair.h" choreonoid-1.1.0+dfsg/include/cnoid/CollisionData000066400000000000000000000000511207742442300217510ustar00rootroot00000000000000#include "src/Collision/CollisionData.h" choreonoid-1.1.0+dfsg/include/cnoid/CollisionPairInserter000066400000000000000000000000611207742442300235100ustar00rootroot00000000000000#include "src/Collision/CollisionPairInserter.h" choreonoid-1.1.0+dfsg/include/cnoid/ComboBox000066400000000000000000000000371207742442300207400ustar00rootroot00000000000000#include "src/Base/ComboBox.h" choreonoid-1.1.0+dfsg/include/cnoid/CompositeIK000066400000000000000000000000421207742442300214120ustar00rootroot00000000000000#include "src/Body/CompositeIK.h" choreonoid-1.1.0+dfsg/include/cnoid/Config000066400000000000000000000000351207742442300204330ustar00rootroot00000000000000#include "src/Util/Config.h" choreonoid-1.1.0+dfsg/include/cnoid/ConnectionSet000066400000000000000000000000441207742442300220010ustar00rootroot00000000000000#include "src/Base/ConnectionSet.h" choreonoid-1.1.0+dfsg/include/cnoid/ConstraintForceSolver000066400000000000000000000000541207742442300235250ustar00rootroot00000000000000#include "src/Body/ConstraintForceSolver.h" choreonoid-1.1.0+dfsg/include/cnoid/DescriptionDialog000066400000000000000000000000561207742442300226340ustar00rootroot00000000000000#include "../../src/Base/DescriptionDialog.h" choreonoid-1.1.0+dfsg/include/cnoid/Dialog000066400000000000000000000000351207742442300204250ustar00rootroot00000000000000#include "src/Base/Dialog.h" choreonoid-1.1.0+dfsg/include/cnoid/EasyScanner000066400000000000000000000000421207742442300214370ustar00rootroot00000000000000#include "src/Util/EasyScanner.h" choreonoid-1.1.0+dfsg/include/cnoid/EigenTypes000066400000000000000000000000411207742442300212770ustar00rootroot00000000000000#include "src/Util/EigenTypes.h" choreonoid-1.1.0+dfsg/include/cnoid/EigenUtil000066400000000000000000000000401207742442300211070ustar00rootroot00000000000000#include "src/Util/EigenUtil.h" choreonoid-1.1.0+dfsg/include/cnoid/EigenYaml000066400000000000000000000000401207742442300210740ustar00rootroot00000000000000#include "src/Util/EigenYaml.h" choreonoid-1.1.0+dfsg/include/cnoid/ExtensionManager000066400000000000000000000000471207742442300225000ustar00rootroot00000000000000#include "src/Base/ExtensionManager.h" choreonoid-1.1.0+dfsg/include/cnoid/FileUtil000066400000000000000000000000451207742442300207440ustar00rootroot00000000000000#include "../../src/Util/FileUtil.h" choreonoid-1.1.0+dfsg/include/cnoid/FolderItem000066400000000000000000000000411207742442300212550ustar00rootroot00000000000000#include "src/Base/FolderItem.h" choreonoid-1.1.0+dfsg/include/cnoid/ForwardDynamics000066400000000000000000000000461207742442300223240ustar00rootroot00000000000000#include "src/Body/ForwardDynamics.h" choreonoid-1.1.0+dfsg/include/cnoid/ForwardDynamicsABM000066400000000000000000000000511207742442300226400ustar00rootroot00000000000000#include "src/Body/ForwardDynamicsABM.h" choreonoid-1.1.0+dfsg/include/cnoid/ForwardDynamicsCBM000066400000000000000000000000511207742442300226420ustar00rootroot00000000000000#include "src/Body/ForwardDynamicsCBM.h" choreonoid-1.1.0+dfsg/include/cnoid/GaussianFilter000066400000000000000000000000451207742442300221470ustar00rootroot00000000000000#include "src/Util/GaussianFilter.h" choreonoid-1.1.0+dfsg/include/cnoid/GraphBar000066400000000000000000000000371207742442300207160ustar00rootroot00000000000000#include "src/Base/GraphBar.h" choreonoid-1.1.0+dfsg/include/cnoid/GraphWidget000066400000000000000000000000421207742442300214310ustar00rootroot00000000000000#include "src/Base/GraphWidget.h" choreonoid-1.1.0+dfsg/include/cnoid/ImageConverter000066400000000000000000000000451207742442300221410ustar00rootroot00000000000000#include "src/Util/ImageConverter.h" choreonoid-1.1.0+dfsg/include/cnoid/InfoBar000066400000000000000000000000361207742442300205470ustar00rootroot00000000000000#include "src/Base/InfoBar.h" choreonoid-1.1.0+dfsg/include/cnoid/Interpolator000066400000000000000000000000431207742442300217070ustar00rootroot00000000000000#include "src/Util/Interpolator.h" choreonoid-1.1.0+dfsg/include/cnoid/InverseKinematics000066400000000000000000000000501207742442300226460ustar00rootroot00000000000000#include "src/Body/InverseKinematics.h" choreonoid-1.1.0+dfsg/include/cnoid/Item000066400000000000000000000000331207742442300201220ustar00rootroot00000000000000#include "src/Base/Item.h" choreonoid-1.1.0+dfsg/include/cnoid/ItemList000066400000000000000000000000371207742442300207620ustar00rootroot00000000000000#include "src/Base/ItemList.h" choreonoid-1.1.0+dfsg/include/cnoid/ItemManager000066400000000000000000000000421207742442300214150ustar00rootroot00000000000000#include "src/Base/ItemManager.h" choreonoid-1.1.0+dfsg/include/cnoid/ItemPath000066400000000000000000000000371207742442300207430ustar00rootroot00000000000000#include "src/Base/ItemPath.h" choreonoid-1.1.0+dfsg/include/cnoid/ItemTreeView000066400000000000000000000000431207742442300215760ustar00rootroot00000000000000#include "src/Base/ItemTreeView.h" choreonoid-1.1.0+dfsg/include/cnoid/JointPath000066400000000000000000000000401207742442300211220ustar00rootroot00000000000000#include "src/Body/JointPath.h" choreonoid-1.1.0+dfsg/include/cnoid/KinematicFaultChecker000066400000000000000000000000621207742442300234130ustar00rootroot00000000000000#include "src/BodyPlugin/KinematicFaultChecker.h" choreonoid-1.1.0+dfsg/include/cnoid/KinematicsBar000066400000000000000000000000521207742442300217410ustar00rootroot00000000000000#include "src/BodyPlugin/KinematicsBar.h" choreonoid-1.1.0+dfsg/include/cnoid/LazyCaller000066400000000000000000000000411207742442300212650ustar00rootroot00000000000000#include "src/Base/LazyCaller.h" choreonoid-1.1.0+dfsg/include/cnoid/LazySignal000066400000000000000000000000411207742442300213000ustar00rootroot00000000000000#include "src/Base/LazySignal.h" choreonoid-1.1.0+dfsg/include/cnoid/LeggedBody000066400000000000000000000000411207742442300212300ustar00rootroot00000000000000#include "src/Body/LeggedBody.h" choreonoid-1.1.0+dfsg/include/cnoid/LineEdit000066400000000000000000000000451207742442300207240ustar00rootroot00000000000000#include "../../src/Base/LineEdit.h" choreonoid-1.1.0+dfsg/include/cnoid/Link000066400000000000000000000000331207742442300201210ustar00rootroot00000000000000#include "src/Body/Link.h" choreonoid-1.1.0+dfsg/include/cnoid/LinkGroup000066400000000000000000000000401207742442300211340ustar00rootroot00000000000000#include "src/Body/LinkGroup.h" choreonoid-1.1.0+dfsg/include/cnoid/LinkPath000066400000000000000000000000371207742442300207420ustar00rootroot00000000000000#include "src/Body/LinkPath.h" choreonoid-1.1.0+dfsg/include/cnoid/LinkSelectionView000066400000000000000000000000561207742442300226270ustar00rootroot00000000000000#include "src/BodyPlugin/LinkSelectionView.h" choreonoid-1.1.0+dfsg/include/cnoid/LinkTraverse000066400000000000000000000000431207742442300216360ustar00rootroot00000000000000#include "src/Body/LinkTraverse.h" choreonoid-1.1.0+dfsg/include/cnoid/LinkTreeWidget000066400000000000000000000000531207742442300221070ustar00rootroot00000000000000#include "src/BodyPlugin/LinkTreeWidget.h" choreonoid-1.1.0+dfsg/include/cnoid/MainWindow000066400000000000000000000000411207742442300212770ustar00rootroot00000000000000#include "src/Base/MainWindow.h" choreonoid-1.1.0+dfsg/include/cnoid/MenuManager000066400000000000000000000000421207742442300214230ustar00rootroot00000000000000#include "src/Base/MenuManager.h" choreonoid-1.1.0+dfsg/include/cnoid/MessageView000066400000000000000000000000421207742442300214430ustar00rootroot00000000000000#include "src/Base/MessageView.h" choreonoid-1.1.0+dfsg/include/cnoid/ModelNodeSet000066400000000000000000000000431207742442300215470ustar00rootroot00000000000000#include "src/Body/ModelNodeSet.h" choreonoid-1.1.0+dfsg/include/cnoid/MultiAffine3Seq000066400000000000000000000000461207742442300221670ustar00rootroot00000000000000#include "src/Util/MultiAffine3Seq.h" choreonoid-1.1.0+dfsg/include/cnoid/MultiAffine3SeqItem000066400000000000000000000000521207742442300230030ustar00rootroot00000000000000#include "src/Base/MultiAffine3SeqItem.h" choreonoid-1.1.0+dfsg/include/cnoid/MultiSeq000066400000000000000000000000371207742442300207730ustar00rootroot00000000000000#include "src/Util/MultiSeq.h" choreonoid-1.1.0+dfsg/include/cnoid/MultiSeqItem000066400000000000000000000000431207742442300216070ustar00rootroot00000000000000#include "src/Base/MultiSeqItem.h" choreonoid-1.1.0+dfsg/include/cnoid/MultiSeqItemCreationPanel000066400000000000000000000000601207742442300242530ustar00rootroot00000000000000#include "src/Base/MultiSeqItemCreationPanel.h" choreonoid-1.1.0+dfsg/include/cnoid/MultiValueSeq000066400000000000000000000000441207742442300217660ustar00rootroot00000000000000#include "src/Util/MultiValueSeq.h" choreonoid-1.1.0+dfsg/include/cnoid/MultiValueSeqItem000066400000000000000000000000501207742442300226020ustar00rootroot00000000000000#include "src/Base/MultiValueSeqItem.h" choreonoid-1.1.0+dfsg/include/cnoid/NullOut000066400000000000000000000000361207742442300206310ustar00rootroot00000000000000#include "src/Util/NullOut.h" choreonoid-1.1.0+dfsg/include/cnoid/OptionManager000066400000000000000000000000441207742442300217710ustar00rootroot00000000000000#include "src/Base/OptionManager.h" choreonoid-1.1.0+dfsg/include/cnoid/OsgNormalVisualizer000066400000000000000000000000521207742442300232040ustar00rootroot00000000000000#include "src/Base/OsgNormalVisualizer.h" choreonoid-1.1.0+dfsg/include/cnoid/OsgOutlineFx000066400000000000000000000000431207742442300216130ustar00rootroot00000000000000#include "src/Base/OsgOutlineFx.h" choreonoid-1.1.0+dfsg/include/cnoid/OsgViewer000066400000000000000000000000401207742442300211340ustar00rootroot00000000000000#include "src/Base/OsgViewer.h" choreonoid-1.1.0+dfsg/include/cnoid/PenetrationBlocker000066400000000000000000000000511207742442300230160ustar00rootroot00000000000000#include "src/Body/PenetrationBlocker.h" choreonoid-1.1.0+dfsg/include/cnoid/PinDragIK000066400000000000000000000000401207742442300207720ustar00rootroot00000000000000#include "src/Body/PinDragIK.h" choreonoid-1.1.0+dfsg/include/cnoid/PlainSeqFormatLoader000066400000000000000000000000531207742442300232420ustar00rootroot00000000000000#include "src/Util/PlainSeqFormatLoader.h" choreonoid-1.1.0+dfsg/include/cnoid/Plugin000066400000000000000000000000351207742442300204640ustar00rootroot00000000000000#include "src/Base/Plugin.h" choreonoid-1.1.0+dfsg/include/cnoid/PluginManager000066400000000000000000000000441207742442300217570ustar00rootroot00000000000000#include "src/Base/PluginManager.h" choreonoid-1.1.0+dfsg/include/cnoid/PoseProvider000066400000000000000000000000431207742442300216460ustar00rootroot00000000000000#include "src/Body/PoseProvider.h" choreonoid-1.1.0+dfsg/include/cnoid/PoseProviderToBodyMotionConverter000066400000000000000000000000701207742442300260450ustar00rootroot00000000000000#include "src/Body/PoseProviderToBodyMotionConverter.h" choreonoid-1.1.0+dfsg/include/cnoid/PoseRollView000066400000000000000000000000541207742442300216210ustar00rootroot00000000000000#include "src/PoseSeqPlugin/PoseRollView.h" choreonoid-1.1.0+dfsg/include/cnoid/ProjectManager000066400000000000000000000000451207742442300221300ustar00rootroot00000000000000#include "src/Base/ProjectManager.h" choreonoid-1.1.0+dfsg/include/cnoid/PutPropertyFunction000066400000000000000000000000521207742442300232500ustar00rootroot00000000000000#include "src/Base/PutPropertyFunction.h" choreonoid-1.1.0+dfsg/include/cnoid/RangeLimiter000066400000000000000000000000431207742442300216070ustar00rootroot00000000000000#include "src/Util/RangeLimiter.h" choreonoid-1.1.0+dfsg/include/cnoid/Referenced000066400000000000000000000000411207742442300212650ustar00rootroot00000000000000#include "src/Util/Referenced.h" choreonoid-1.1.0+dfsg/include/cnoid/RootItem000066400000000000000000000000371207742442300207720ustar00rootroot00000000000000#include "src/Base/RootItem.h" choreonoid-1.1.0+dfsg/include/cnoid/SceneBody000066400000000000000000000000461207742442300211030ustar00rootroot00000000000000#include "src/BodyPlugin/SceneBody.h" choreonoid-1.1.0+dfsg/include/cnoid/SceneBodyManager000066400000000000000000000000551207742442300223760ustar00rootroot00000000000000#include "src/BodyPlugin/SceneBodyManager.h" choreonoid-1.1.0+dfsg/include/cnoid/SceneGL000066400000000000000000000000441207742442300205060ustar00rootroot00000000000000#include "../../src/Base/SceneGL.h" choreonoid-1.1.0+dfsg/include/cnoid/SceneItem000066400000000000000000000000401207742442300210760ustar00rootroot00000000000000#include "src/Base/SceneItem.h" choreonoid-1.1.0+dfsg/include/cnoid/SceneObject000066400000000000000000000000421207742442300214100ustar00rootroot00000000000000#include "src/Base/SceneObject.h" choreonoid-1.1.0+dfsg/include/cnoid/ScenePieces000066400000000000000000000000421207742442300214120ustar00rootroot00000000000000#include "src/Base/ScenePieces.h" choreonoid-1.1.0+dfsg/include/cnoid/SceneView000066400000000000000000000000401207742442300211120ustar00rootroot00000000000000#include "src/Base/SceneView.h" choreonoid-1.1.0+dfsg/include/cnoid/SceneWorld000066400000000000000000000000471207742442300212760ustar00rootroot00000000000000#include "src/BodyPlugin/SceneWorld.h" choreonoid-1.1.0+dfsg/include/cnoid/ScrollBar000066400000000000000000000000401207742442300211050ustar00rootroot00000000000000#include "src/Base/ScrollBar.h" choreonoid-1.1.0+dfsg/include/cnoid/Sensor000066400000000000000000000000351207742442300204770ustar00rootroot00000000000000#include "src/Body/Sensor.h" choreonoid-1.1.0+dfsg/include/cnoid/Separator000066400000000000000000000000401207742442300211620ustar00rootroot00000000000000#include "src/Base/Separator.h" choreonoid-1.1.0+dfsg/include/cnoid/Seq000066400000000000000000000000321207742442300177530ustar00rootroot00000000000000#include "src/Util/Seq.h" choreonoid-1.1.0+dfsg/include/cnoid/SeqBase000066400000000000000000000000361207742442300205520ustar00rootroot00000000000000#include "src/Util/SeqBase.h" choreonoid-1.1.0+dfsg/include/cnoid/SignalProxy000066400000000000000000000000421207742442300215030ustar00rootroot00000000000000#include "src/Base/SignalProxy.h" choreonoid-1.1.0+dfsg/include/cnoid/SimulatorItem000066400000000000000000000000521207742442300220230ustar00rootroot00000000000000#include "src/BodyPlugin/SimulatorItem.h" choreonoid-1.1.0+dfsg/include/cnoid/Sleep000066400000000000000000000000341207742442300202750ustar00rootroot00000000000000#include "src/Util/Sleep.h" choreonoid-1.1.0+dfsg/include/cnoid/Slider000066400000000000000000000000351207742442300204500ustar00rootroot00000000000000#include "src/Base/Slider.h" choreonoid-1.1.0+dfsg/include/cnoid/SpinBox000066400000000000000000000000361207742442300206110ustar00rootroot00000000000000#include "src/Base/SpinBox.h" choreonoid-1.1.0+dfsg/include/cnoid/TimeBar000066400000000000000000000000361207742442300205520ustar00rootroot00000000000000#include "src/Base/TimeBar.h" choreonoid-1.1.0+dfsg/include/cnoid/TimeMeasure000066400000000000000000000000421207742442300214440ustar00rootroot00000000000000#include "src/Util/TimeMeasure.h" choreonoid-1.1.0+dfsg/include/cnoid/TimeSyncItemEngineManager000066400000000000000000000000601207742442300242170ustar00rootroot00000000000000#include "src/Base/TimeSyncItemEngineManager.h" choreonoid-1.1.0+dfsg/include/cnoid/ToolBar000066400000000000000000000000361207742442300205710ustar00rootroot00000000000000#include "src/Base/ToolBar.h" choreonoid-1.1.0+dfsg/include/cnoid/TreeView000066400000000000000000000000371207742442300207620ustar00rootroot00000000000000#include "src/Base/TreeView.h" choreonoid-1.1.0+dfsg/include/cnoid/TreeWidget000066400000000000000000000000411207742442300212660ustar00rootroot00000000000000#include "src/Base/TreeWidget.h" choreonoid-1.1.0+dfsg/include/cnoid/TriangleMeshShaper000066400000000000000000000000511207742442300227510ustar00rootroot00000000000000#include "src/Util/TriangleMeshShaper.h" choreonoid-1.1.0+dfsg/include/cnoid/Triangulator000066400000000000000000000000431207742442300217000ustar00rootroot00000000000000#include "src/Util/Triangulator.h" choreonoid-1.1.0+dfsg/include/cnoid/Utf8000066400000000000000000000000411207742442300200510ustar00rootroot00000000000000#include "../../src/Util/Utf8.h" choreonoid-1.1.0+dfsg/include/cnoid/Vector3Seq000066400000000000000000000000411207742442300212210ustar00rootroot00000000000000#include "src/Util/Vector3Seq.h" choreonoid-1.1.0+dfsg/include/cnoid/Vector3SeqItem000066400000000000000000000000451207742442300220440ustar00rootroot00000000000000#include "src/Base/Vector3SeqItem.h" choreonoid-1.1.0+dfsg/include/cnoid/View000066400000000000000000000000331207742442300201360ustar00rootroot00000000000000#include "src/Base/View.h" choreonoid-1.1.0+dfsg/include/cnoid/VrmlNodes000066400000000000000000000000401207742442300211330ustar00rootroot00000000000000#include "src/Util/VrmlNodes.h" choreonoid-1.1.0+dfsg/include/cnoid/VrmlParser000066400000000000000000000000411207742442300213200ustar00rootroot00000000000000#include "src/Util/VrmlParser.h" choreonoid-1.1.0+dfsg/include/cnoid/VrmlToOsgConverter000066400000000000000000000000511207742442300230100ustar00rootroot00000000000000#include "src/Base/VrmlToOsgConverter.h" choreonoid-1.1.0+dfsg/include/cnoid/VrmlWriter000066400000000000000000000000411207742442300213400ustar00rootroot00000000000000#include "src/Util/VrmlWriter.h" choreonoid-1.1.0+dfsg/include/cnoid/World000066400000000000000000000000341207742442300203140ustar00rootroot00000000000000#include "src/Body/World.h" choreonoid-1.1.0+dfsg/include/cnoid/WorldItem000066400000000000000000000000461207742442300211360ustar00rootroot00000000000000#include "src/BodyPlugin/WorldItem.h" choreonoid-1.1.0+dfsg/include/cnoid/YamlNodes000066400000000000000000000000401207742442300211150ustar00rootroot00000000000000#include "src/Util/YamlNodes.h" choreonoid-1.1.0+dfsg/include/cnoid/YamlReader000066400000000000000000000000411207742442300212500ustar00rootroot00000000000000#include "src/Util/YamlReader.h" choreonoid-1.1.0+dfsg/include/cnoid/YamlWriter000066400000000000000000000000411207742442300213220ustar00rootroot00000000000000#include "src/Util/YamlWriter.h" choreonoid-1.1.0+dfsg/misc/000077500000000000000000000000001207742442300155215ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/misc/CMakeLists.txt000066400000000000000000000000351207742442300202570ustar00rootroot00000000000000 add_subdirectory(pkgconfig) choreonoid-1.1.0+dfsg/misc/nsis/000077500000000000000000000000001207742442300164755ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/misc/nsis/FileAssociation.nsh000066400000000000000000000113451207742442300222670ustar00rootroot00000000000000/* _____________________________________________________________________________ File Association _____________________________________________________________________________ Based on code taken from http://nsis.sourceforge.net/File_Association Usage in script: 1. !include "FileAssociation.nsh" 2. [Section|Function] ${FileAssociationFunction} "Param1" "Param2" "..." $var [SectionEnd|FunctionEnd] FileAssociationFunction=[RegisterExtension|UnRegisterExtension] _____________________________________________________________________________ ${RegisterExtension} "[executable]" "[extension]" "[description]" "[executable]" ; executable which opens the file format ; "[extension]" ; extension, which represents the file format to open ; "[description]" ; description for the extension. This will be display in Windows Explorer. ; ${UnRegisterExtension} "[extension]" "[description]" "[extension]" ; extension, which represents the file format to open ; "[description]" ; description for the extension. This will be display in Windows Explorer. ; _____________________________________________________________________________ Macros _____________________________________________________________________________ Change log window verbosity (default: 3=no script) Example: !include "FileAssociation.nsh" !insertmacro RegisterExtension ${FileAssociation_VERBOSE} 4 # all verbosity !insertmacro UnRegisterExtension ${FileAssociation_VERBOSE} 3 # no script */ !ifndef FileAssociation_INCLUDED !define FileAssociation_INCLUDED !include Util.nsh !verbose push !verbose 3 !ifndef _FileAssociation_VERBOSE !define _FileAssociation_VERBOSE 3 !endif !verbose ${_FileAssociation_VERBOSE} !define FileAssociation_VERBOSE `!insertmacro FileAssociation_VERBOSE` !verbose pop !macro FileAssociation_VERBOSE _VERBOSE !verbose push !verbose 3 !undef _FileAssociation_VERBOSE !define _FileAssociation_VERBOSE ${_VERBOSE} !verbose pop !macroend !macro RegisterExtensionCall _EXECUTABLE _EXTENSION _DESCRIPTION !verbose push !verbose ${_FileAssociation_VERBOSE} Push `${_DESCRIPTION}` Push `${_EXTENSION}` Push `${_EXECUTABLE}` ${CallArtificialFunction} RegisterExtension_ !verbose pop !macroend !macro UnRegisterExtensionCall _EXTENSION _DESCRIPTION !verbose push !verbose ${_FileAssociation_VERBOSE} Push `${_EXTENSION}` Push `${_DESCRIPTION}` ${CallArtificialFunction} UnRegisterExtension_ !verbose pop !macroend !define RegisterExtension `!insertmacro RegisterExtensionCall` !define un.RegisterExtension `!insertmacro RegisterExtensionCall` !macro RegisterExtension !macroend !macro un.RegisterExtension !macroend !macro RegisterExtension_ !verbose push !verbose ${_FileAssociation_VERBOSE} Exch $R2 ;exe Exch Exch $R1 ;ext Exch Exch 2 Exch $R0 ;desc Exch 2 Push $0 Push $1 ReadRegStr $1 HKCR $R1 "" ; read current file association StrCmp "$1" "" NoBackup ; is it empty StrCmp "$1" "$R0" NoBackup ; is it our own WriteRegStr HKCR $R1 "backup_val" "$1" ; backup current value NoBackup: WriteRegStr HKCR $R1 "" "$R0" ; set our file association ReadRegStr $0 HKCR $R0 "" StrCmp $0 "" 0 Skip WriteRegStr HKCR "$R0" "" "$R0" WriteRegStr HKCR "$R0\shell" "" "open" WriteRegStr HKCR "$R0\DefaultIcon" "" "$R2,0" Skip: WriteRegStr HKCR "$R0\shell\open\command" "" '"$R2" "%1"' WriteRegStr HKCR "$R0\shell\edit" "" "Edit $R0" WriteRegStr HKCR "$R0\shell\edit\command" "" '"$R2" "%1"' Pop $1 Pop $0 Pop $R2 Pop $R1 Pop $R0 !verbose pop !macroend !define UnRegisterExtension `!insertmacro UnRegisterExtensionCall` !define un.UnRegisterExtension `!insertmacro UnRegisterExtensionCall` !macro UnRegisterExtension !macroend !macro un.UnRegisterExtension !macroend !macro UnRegisterExtension_ !verbose push !verbose ${_FileAssociation_VERBOSE} Exch $R1 ;desc Exch Exch $R0 ;ext Exch Push $0 Push $1 ReadRegStr $1 HKCR $R0 "" StrCmp $1 $R1 0 NoOwn ; only do this if we own it ReadRegStr $1 HKCR $R0 "backup_val" StrCmp $1 "" 0 Restore ; if backup="" then delete the whole key DeleteRegKey HKCR $R0 Goto NoOwn Restore: WriteRegStr HKCR $R0 "" $1 DeleteRegValue HKCR $R0 "backup_val" DeleteRegKey HKCR $R1 ;Delete key with association name settings NoOwn: Pop $1 Pop $0 Pop $R1 Pop $R0 !verbose pop !macroend !endif # !FileAssociation_INCLUDED choreonoid-1.1.0+dfsg/misc/pkgconfig/000077500000000000000000000000001207742442300174705ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/misc/pkgconfig/CMakeLists.txt000066400000000000000000000004111207742442300222240ustar00rootroot00000000000000 foreach(file choreonoid.pc choreonoid-body-plugin.pc) configure_file(${file}.in ${PROJECT_BINARY_DIR}/lib/pkgconfig/${file} @ONLY) if(INSTALL_SDK) install(FILES ${PROJECT_BINARY_DIR}/lib/pkgconfig/${file} DESTINATION lib/pkgconfig) endif() endforeach() choreonoid-1.1.0+dfsg/misc/pkgconfig/choreonoid-body-plugin.pc.in000066400000000000000000000003301207742442300247750ustar00rootroot00000000000000# pkg-config source file Name: choreonoid-body-plugin Description: Body plugin of choreonoid Version: @CNOID_FULL_VERSION@ Requires: choreonoid >= @CNOID_FULL_VERSION@ Conflicts: Libs: -lCnoidBody -lCnoidBodyPlugin choreonoid-1.1.0+dfsg/misc/pkgconfig/choreonoid.pc.in000066400000000000000000000007411207742442300225540ustar00rootroot00000000000000# pkg-config source file prefix=@CMAKE_INSTALL_PREFIX@ exec_prefix=${prefix} libdir=${exec_prefix}/lib plugindir=${exec_prefix}/@CNOID_PLUGIN_SUBDIR@ includedir=${prefix}/@CNOID_HEADER_SUBDIR@ Name: choreonoid Description: Robotics application framework Version: @CNOID_FULL_VERSION@ Requires: QtCore >= 4.7.0 QtGui >= 4.7.0 QtOpenGL >= 4.7.0 openscenegraph >= 2.8.0 Conflicts: Libs: -L${libdir} -L${plugindir} -lCnoidUtil -lCnoidGuiBase Cflags: -I${includedir} -DQT_NO_KEYWORDS choreonoid-1.1.0+dfsg/misc/script/000077500000000000000000000000001207742442300170255ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/misc/script/extract-messages-initial.sh000077500000000000000000000005061207742442300242730ustar00rootroot00000000000000#!/bin/sh if [ ! -d "po" ]; then mkdir po fi echo "Extracting messages to translate from the source files." xgettext -k"_" -k"N_" -k"Q_" -o po/messages.pot *.cpp if [ -n "$1" ]; then echo "Creating $1.po file." msginit --input=po/messages.pot --locale=$1 -o po/$1.po else echo "Please specify a locale." fi choreonoid-1.1.0+dfsg/misc/script/extract-messages.sh000077500000000000000000000003541207742442300226450ustar00rootroot00000000000000#!/bin/sh echo "Extracting messages to translate from the source files." xgettext -k"_" -k"N_" -k"Q_" -o po/messages.pot *.cpp for po in po/*.po do echo "Merging new messages to $po." msgmerge --update $po po/messages.pot done choreonoid-1.1.0+dfsg/misc/script/install-requisites-ubuntu.sh000077500000000000000000000005001207742442300245400ustar00rootroot00000000000000#!/bin/sh sudo apt-get install \ build-essential \ cmake-curses-gui \ doxygen \ graphviz \ libboost-all-dev \ zlib1g-dev \ libjpeg62-dev \ libpng12-dev \ libopenscenegraph-dev \ openscenegraph-doc \ libsimage-dev \ libyaml-dev \ libqt4-dev \ libqt4-opengl-dev \ qt4-dev-tools \ qt4-qtconfig \ qt4-doc-html \ gettext choreonoid-1.1.0+dfsg/misc/script/make_header_public.rb000077500000000000000000000016421207742442300231430ustar00rootroot00000000000000#!/usr/bin/ruby require 'pathname' def make_pathname_from_include(orgPath) path = Pathname(orgPath) while true path = path.parent if path.basename.to_s == "include" return Pathname(orgPath).relative_path_from(path) end break if path.root? end return nil end n = ARGV.size - 1 if n <= 0 puts "make_header_public.rb header-files ... directory-to-link-headers" exit end output_directory = File.expand_path(ARGV[n], Dir.pwd) output_pathname = Pathname(output_directory) for i in n.times orgFileName = ARGV[i] linkFilePath = (Pathname(output_directory) + Pathname(orgFileName).basename(".h")).to_s orgPathName = Pathname(File.expand_path(orgFileName, Dir.pwd)) relpath = orgPathName.relative_path_from(output_pathname) print "make public header <" + make_pathname_from_include(linkFilePath).to_s + ">\n" file = open(linkFilePath, "w") file.print "\#include \"#{relpath}\"\n" end choreonoid-1.1.0+dfsg/share/000077500000000000000000000000001207742442300156705ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/CMakeLists.txt000066400000000000000000000002451207742442300204310ustar00rootroot00000000000000 install(DIRECTORY models projects DESTINATION ${CNOID_SHARE_SUBDIR}) if(INSTALL_SDK) install(DIRECTORY sampleplugins DESTINATION ${CNOID_SHARE_SUBDIR}) endif() choreonoid-1.1.0+dfsg/share/models/000077500000000000000000000000001207742442300171535ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/models/GR001/000077500000000000000000000000001207742442300177045ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/models/GR001/GR001.wrl000066400000000000000000000516671207742442300212020ustar00rootroot00000000000000#VRML V2.0 utf8 PROTO Joint [ exposedField SFVec3f center 0 0 0 exposedField MFNode children [] exposedField MFFloat llimit [] exposedField MFFloat lvlimit [] exposedField SFRotation limitOrientation 0 0 1 0 exposedField SFString name "" exposedField SFRotation rotation 0 0 1 0 exposedField SFVec3f scale 1 1 1 exposedField SFRotation scaleOrientation 0 0 1 0 exposedField MFFloat stiffness [ 0 0 0 ] exposedField SFVec3f translation 0 0 0 exposedField MFFloat ulimit [] exposedField MFFloat uvlimit [] exposedField SFString jointType "" exposedField SFInt32 jointId -1 exposedField SFVec3f jointAxis 0 0 1 exposedField SFFloat gearRatio 1 exposedField SFFloat rotorInertia 0 exposedField SFFloat rotorResistor 0 exposedField SFFloat torqueConst 1 exposedField SFFloat encoderPulse 1 ] { Transform { center IS center children IS children rotation IS rotation scale IS scale scaleOrientation IS scaleOrientation translation IS translation } } PROTO Segment [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 exposedField SFVec3f centerOfMass 0 0 0 exposedField MFNode children [ ] exposedField SFNode coord NULL exposedField MFNode displacers [ ] exposedField SFFloat mass 0 exposedField MFFloat momentsOfInertia [ 0 0 0 0 0 0 0 0 0 ] exposedField SFString name "" eventIn MFNode addChildren eventIn MFNode removeChildren ] { Group { addChildren IS addChildren bboxCenter IS bboxCenter bboxSize IS bboxSize children IS children removeChildren IS removeChildren } } PROTO Humanoid [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 exposedField SFVec3f center 0 0 0 exposedField MFNode humanoidBody [ ] exposedField MFString info [ ] exposedField MFNode joints [ ] exposedField SFString name "" exposedField SFRotation rotation 0 0 1 0 exposedField SFVec3f scale 1 1 1 exposedField SFRotation scaleOrientation 0 0 1 0 exposedField MFNode segments [ ] exposedField MFNode sites [ ] exposedField SFVec3f translation 0 0 0 exposedField SFString version "1.1" exposedField MFNode viewpoints [ ] ] { Transform { bboxCenter IS bboxCenter bboxSize IS bboxSize center IS center rotation IS rotation scale IS scale scaleOrientation IS scaleOrientation translation IS translation children [ Group { children IS viewpoints } Group { children IS humanoidBody } ] } } PROTO VisionSensor [ exposedField SFVec3f translation 0 0 0 exposedField SFRotation rotation 0 0 1 0 exposedField MFNode children [ ] exposedField SFFloat fieldOfView 0.785398 exposedField SFString name "" exposedField SFFloat frontClipDistance 0.01 exposedField SFFloat backClipDistance 10.0 exposedField SFString type "NONE" exposedField SFInt32 sensorId -1 exposedField SFInt32 width 320 exposedField SFInt32 height 240 exposedField SFFloat frameRate 30 ] { Transform { rotation IS rotation translation IS translation children IS children } } PROTO ForceSensor [ exposedField SFVec3f maxForce -1 -1 -1 exposedField SFVec3f maxTorque -1 -1 -1 exposedField SFVec3f translation 0 0 0 exposedField SFRotation rotation 0 0 1 0 exposedField MFNode children [ ] exposedField SFInt32 sensorId -1 ] { Transform { translation IS translation rotation IS rotation children IS children } } PROTO Gyro [ exposedField SFVec3f maxAngularVelocity -1 -1 -1 exposedField SFVec3f translation 0 0 0 exposedField SFRotation rotation 0 0 1 0 exposedField MFNode children [ ] exposedField SFInt32 sensorId -1 ] { Transform { translation IS translation rotation IS rotation children IS children } } PROTO AccelerationSensor [ exposedField SFVec3f maxAcceleration -1 -1 -1 exposedField SFVec3f translation 0 0 0 exposedField SFRotation rotation 0 0 1 0 exposedField MFNode children [ ] exposedField SFInt32 sensorId -1 ] { Transform { translation IS translation rotation IS rotation children IS children } } DEF GR001 Humanoid{ humanoidBody [ DEF WAIST Joint { jointType "free" translation 0 0 0.1605 children[ DEF WAIST_LINK Segment{ centerOfMass 0.014070775509439625 -0.0010642631392533723 0.018082091556018958 mass 0.16852 momentsOfInertia[ 7.5307790665527126e-05 -1.7122423225418951e-06 -7.9600885666462265e-06 -1.7122423225418951e-06 9.8462174358634891e-05 1.6233365559722733e-06 -7.9600885666462265e-06 1.6233365559722733e-06 0.00012629712705606254 ] children[ Inline { url "parts/WAIST.wrl" } ] } DEF R_HIP_Y Joint { jointType "rotate" jointId 0 jointAxis 0.0 0.0 1.0 translation 0 -0.0221 0 ulimit [0.524 ] llimit [-2.618 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_HIP_Y_LINK Segment{ centerOfMass 0.0098738158196746834, 0.0019351632042820921, -0.010641978258911932 mass 0.01075 momentsOfInertia [ 1.9516618643475393e-06 -4.4794177287632572e-07 4.8697160385938456e-07 -4.4794177287632572e-07 5.0103304284609765e-06 1.1632114977313334e-08 4.8697160385938456e-07 1.1632114977313334e-08 5.440099096984392e-06 ] children[ Inline { url "parts/R_HIP_Y.wrl" } ] } DEF R_HIP_R Joint { jointType "rotate" jointId 1 jointAxis -1 0 0 translation 0 0.0025 -0.028 ulimit [1.571 ] llimit [-1.571 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_HIP_R_LINK Segment{ centerOfMass 0.013202756254495145 5.5007249363651409e-05 -0.0093736922025596577 mass 0.06587 momentsOfInertia [ 1.1527391880654788e-05 3.3279576102840693e-07 1.8461834995431493e-06 3.3279576102840693e-07 1.933034542735627e-05 -1.0226135519064636e-07 1.8461834995431493e-06 -1.0226135519064636e-07 1.4225997781058221e-05 ] children[ Inline { url "parts/R_HIP_R.wrl" } ] } DEF R_HIP_P Joint { jointType "rotate" jointId 2 jointAxis 0 -1 0 translation 0.029 0.0 -0.005 ulimit [2.094 ] llimit [-0.698 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_HIP_P_LINK Segment{ centerOfMass 0.012315059064213048 -0.0026831832924373755 -0.036400277402077907 mass 0.04249 momentsOfInertia[ 1.082813917097787e-05 -2.7126598481517409e-07 1.8370007325714608e-07 -2.7126598481517409e-07 1.1393542083797361e-05 8.7593994599052584e-07 1.8370007325714608e-07 8.7593994599052584e-07 8.3880053868115751e-06 ] children[ Inline { url "parts/R_HIP_P.wrl" } ] } DEF R_KNEE_P Joint { jointType "rotate" jointId 3 jointAxis 0 -1 0 translation 0 0 -0.048 ulimit [0.0 ] llimit [-2.269 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_KNEE_P_LINK Segment{ centerOfMass 0.0054334208830912611 -0.0045557250577152588 -0.022327982304852591 mass 0.01307 momentsOfInertia [ 7.8215085179879079e-06 -1.9125278185636929e-07 -8.0678174194219455e-07 -1.9125278185636929e-07 5.3468754380240647e-06 -3.1722504110141287e-07 -8.0678174194219455e-07 -3.1722504110141287e-07 3.7864977034795684e-06 ] children[ Inline { url "parts/R_KNEE_P.wrl" } ] } DEF R_ANKLE_P Joint { jointType "rotate" jointId 4 jointAxis 0 1 0 translation 0 0 -0.059 ulimit [1.047 ] llimit [-1.658 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_ANKLE_P_LINK Segment{ centerOfMass -0.010514983772173681 -0.00092494740382487032 0.006396772110621252 mass 0.0656 momentsOfInertia [ 1.1506271948838844e-05 3.6112601146108375e-07 -1.0960578172280234e-08 3.6112601146108375e-07 2.1019676061582625e-05 2.8207268765248394e-08 -1.0960578172280234e-08 2.8207268765248394e-08 1.5933452986563463e-05 ] children[ Inline { url "parts/R_ANKLE_P.wrl" } ] } DEF R_ANKLE_R Joint { jointType "rotate" jointId 5 jointAxis 1 0 0 translation 0 0 0.0005 ulimit [1.571 ] llimit [-0.785 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_ANKLE_R_LINK Segment{ centerOfMass -0.012299329345648103 -0.011256454634381104 -0.018194268448920348 mass 0.01792 momentsOfInertia[ 5.102862742548993e-06 -1.4311400561157794e-08 -1.2500454265702693e-08 -1.4311400561157794e-08 1.1932351766822529e-05 -4.1556885057173572e-07 -1.2500454265702693e-08 -4.1556885057173572e-07 1.5962207274521142e-05 ] children[ Inline { url "parts/R_ANKLE_R.wrl" } ] } ] } ] } ] } ] } ] } ] } DEF L_HIP_Y Joint { jointType "rotate" jointId 6 jointAxis 0.0 0.0 1.0 translation 0 0.0221 0 ulimit [2.618 ] llimit [-0.524 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF L_HIP_Y_LINK Segment{ centerOfMass 0.0098738158196747094, -0.0019351632042820964, -0.008641978258911916 mass 0.01075 momentsOfInertia [ 1.9516618643475554e-06 4.4794177287632705e-07 4.8697160385938657e-07 4.4794177287632705e-07 5.0103304284609951e-06 -1.1632114977312446e-08 4.8697160385938657e-07 -1.1632114977312446e-08 5.440099096984425e-06 ] children[ Inline { url "parts/L_HIP_Y.wrl" } ] } DEF L_HIP_R Joint { jointType "rotate" jointId 7 jointAxis -1 0 0 translation 0 -0.0025 -0.028 ulimit [1.571 ] llimit [-1.571 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ Transform { rotation 0 1 0 0 children[ DEF L_HIP_R_LINK Segment{ centerOfMass 0.013202756254495143 -5.5007249363677958e-05 -0.0093736922025596525 mass 0.06587 momentsOfInertia [ 1.1527391880654793e-05 -3.3279576102840529e-07 1.846183499543155e-06 -3.3279576102840529e-07 1.9330345427356294e-05 1.022613551906479e-07 1.846183499543155e-06 1.022613551906479e-07 1.4225997781058231e-05 ] children[ Inline { url "parts/L_HIP_R.wrl" } ] } ] } DEF L_HIP_P Joint { jointType "rotate" jointId 8 jointAxis 0 1 0 translation 0.029 0.0 -0.005 ulimit [ 0.698 ] llimit [-2.094 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF L_HIP_P_LINK Segment{ centerOfMass 0.012315059064213051 0.0026831832924373968 -0.036400277402077907 mass 0.04249 momentsOfInertia [ 1.0828139170977909e-05 2.7126598481517499e-07 1.8370007325714884e-07 2.7126598481517499e-07 1.139354208379742e-05 -8.7593994599052404e-07 1.8370007325714884e-07 -8.7593994599052404e-07 8.3880053868115531e-06 ] children[ Inline { url "parts/L_HIP_P.wrl" } ] } DEF L_KNEE_P Joint { jointType "rotate" jointId 9 jointAxis 0 1 0 translation 0 0 -0.048 ulimit [2.269 ] llimit [0.0] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF L_KNEE_P_LINK Segment{ centerOfMass 0.0050944529332231933 0.0052148671697826937 -0.022550612186734664 mass 0.01307 momentsOfInertia [ 7.7867091563422099e-06 2.6555643813326173e-07 -8.1173670094028792e-07 2.6555643813326173e-07 5.2539557271461073e-06 3.6707383373694058e-07 -8.1173670094028792e-07 3.6707383373694058e-07 3.8741968358549516e-06 ] children[ Inline { url "parts/L_KNEE_P.wrl" } ] } DEF L_ANKLE_P Joint { jointType "rotate" jointId 10 jointAxis 0 -1 0 translation 0 0 -0.059 ulimit [1.658 ] llimit [-1.047 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF L_ANKLE_P_LINK Segment{ centerOfMass -0.010514983772173688 0.0009249473053786659 0.0063967712285226755 mass 0.0656 momentsOfInertia [ 1.1506271948838858e-05 -3.6112602191375958e-07 -1.096023377566418e-08 -3.6112602191375958e-07 2.1019676007776897e-05 -2.8212119365793259e-08 -1.096023377566418e-08 -2.8212119365793259e-08 1.5933453040369218e-05 ] children[ Inline { url "parts/L_ANKLE_P.wrl" } ] } DEF L_ANKLE_R Joint { jointType "rotate" jointId 11 jointAxis 1 0 0 translation 0 0 0.0005 ulimit [0.785 ] llimit [-1.571 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF L_ANKLE_R_LINK Segment{ centerOfMass -0.012299329345648083 0.011256454634381099 -0.018194268448920352 mass 0.01792 momentsOfInertia [ 5.102862742548993e-06 -1.4311400561157794e-08 -1.2500454265702693e-08 -1.4311400561157794e-08 1.1932351766822529e-05 -4.1556885057173572e-07 -1.2500454265702693e-08 -4.1556885057173572e-07 1.5962207274521142e-05 ] children[ Inline { url "parts/L_ANKLE_R.wrl" } ] } ] } ] } ] } ] } ] } ] } DEF CHEST_P Joint { jointType "rotate" jointId 12 jointAxis 0 -1 0 translation 0.044 0.0 0.02 ulimit [0.0] llimit [-1.658 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF CHEST_P_LINK Segment{ centerOfMass -0.020730842888781902 0.00025411885296551366 0.042966477725380772 mass 0.12824 momentsOfInertia [ 7.4792345496270588e-05 -2.0460700160580106e-08 2.6612836351540438e-06 -2.0460700160580106e-08 5.6631637624553763e-05 1.5956077735064471e-08 2.6612836351540438e-06 1.5956077735064471e-08 0.0001061030019683906 ] children[ Inline { url "parts/CHEST_P.wrl" } ] } DEF NECK_Y Joint { jointType "rotate" jointId 13 jointAxis 0.0 0.0 -1.0 translation -0.012 0 0.056 ulimit [0.8720000000000003 ] llimit [-0.8720000000000003 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF NECK_Y_LINK Segment{ centerOfMass 2.2786043818985748E-5 5.3290705182007515E-18 0.008215358964843369 mass 0.00527 momentsOfInertia [ 4.6330586214015664e-07 -1.8031706925116486e-22 -5.8734859937494843e-10 -1.8031706925116486e-22 4.9575960481670661e-07 -2.7047560387674729e-22 -5.8734859937494843e-10 -2.7047560387674729e-22 6.7145032800772607e-07 ] children[ Inline { url "parts/NECK_Y.wrl" } ] } ] } DEF R_SHOULDER_P Joint { jointType "rotate" jointId 14 jointAxis 0 1 0 translation -0.01 -0.041 0.042 ulimit [2.618 ] llimit [-2.618 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_SHOULDER_P_LINK Segment{ centerOfMass -0.0013812847067215142 -0.010516066209780376 -0.0029935461211070872 mass 0.00975 momentsOfInertia [ 1.3095800287113333e-06 2.1515967001874234e-08 -3.458271618147783e-08 2.1515967001874234e-08 2.6327935898039409e-06 -1.0994626230711128e-07 -3.458271618147783e-08 -1.0994626230711128e-07 2.7200506260844207e-06 ] children[ Inline { url "parts/R_SHOULDER_P.wrl" } ] } DEF R_SHOULDER_R Joint { jointType "rotate" jointId 15 jointAxis 1 0 0 translation 0 -0.025 -0.008 ulimit [0.698 ] llimit [-2.618 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_SHOULDER_R_LINK Segment{ centerOfMass -0.0035468902411903828 -0.0015567363057332911 -0.014437898304582529 mass 0.03686 momentsOfInertia [ 4.9508840880307424e-06 8.134138909631631e-08 -1.307404018922331e-07 8.134138909631631e-08 9.9533099200177678e-06 -4.1565325421949963e-07 -1.307404018922331e-07 -4.1565325421949963e-07 1.0283186264356074e-05 ] children[ Inline { url "parts/R_SHOULDER_R.wrl" } ] } DEF R_ELBOW_P Joint { jointType "rotate" jointId 16 jointAxis 0 1 0 # translation 0.009 0 -0.045 translation 0.009 0 -0.047 # by Hara ulimit [0.873 ] llimit [-2.269 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF R_ELBOW_P_LINK Segment{ centerOfMass 0.0015461746197517063 0.00033795855263700454 -0.015625257449077206 mass 0.02905 momentsOfInertia [ 1.0789525167996382e-05 3.9918558722109871e-09 1.7978343794464902e-06 3.9918558722109871e-09 1.1003152443935253e-05 -4.0046039702229187e-08 1.7978343794464902e-06 -4.0046039702229187e-08 3.2591072507697875e-06 ] children[ Inline { url "parts/R_ELBOW_P.wrl" } ] } ] } ] } ] } DEF L_SHOULDER_P Joint { jointType "rotate" jointId 17 jointAxis 0 -1 .0 translation -0.01 0.041 0.042 ulimit [2.618 ] llimit [-2.618 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF L_SHOULDER_P_LINK Segment{ centerOfMass -0.0013812821724981078 0.010516052694166637 -0.0029935523353056348 mass 0.00975 momentsOfInertia [ 1.3095800142021788e-06 -2.1515553828179759e-08 -3.4582658591096929e-08 -2.1515553828179759e-08 2.6327938140190386e-06 1.0994635718211764e-07 -3.4582658591096929e-08 1.0994635718211764e-07 2.7200504163784756e-06 ] children[ Inline { url "parts/L_SHOULDER_P.wrl" } ] } DEF L_SHOULDER_R Joint { jointType "rotate" jointId 18 jointAxis 1 0 0 translation 0 0.025 -0.008 ulimit [2.618 ] llimit [-0.698 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF L_SHOULDER_R_LINK Segment{ centerOfMass -0.0035468902411903828 0.0015567363057332922 -0.014437898304582539 mass 0.03686 momentsOfInertia [ 1.0694950140451039e-05 1.9723127905453326e-07 -2.6065897076445542e-07 1.9723127905453326e-07 1.0052420713671298e-05 4.8438918432883336e-08 -2.6065897076445542e-07 4.8438918432883336e-08 4.8379288533730014e-06 ] children[ Inline { url "parts/L_SHOULDER_R.wrl" } ] } DEF L_ELBOW_P Joint { jointType "rotate" jointId 19 jointAxis 0 -1 0 translation 0.009 0 -0.047 ulimit [2.269 ] llimit [-0.873 ] uvlimit [ ] lvlimit [ ] rotorInertia 1.0 rotorResistor 1.0 children[ DEF L_ELBOW_P_LINK Segment{ centerOfMass 0.0015461746197516959 -0.00033796007273481838 -0.015625257207349728 mass 0.02905 momentsOfInertia [ 1.0789525167996373e-05 -3.9905699607782306e-09 1.7978343823012256e-06 -3.9905699607782306e-09 1.1003152501217637e-05 4.0040500729103886e-08 1.7978343823012256e-06 4.0040500729103886e-08 3.259107193487409e-06 ] children[ Inline { url "parts/L_ELBOW_P.wrl" } ] } ] } ] } ] } ] } ] } ] joints [ USE WAIST, USE R_HIP_Y, USE R_HIP_R, USE R_HIP_P, USE R_KNEE_P, USE R_ANKLE_P, USE R_ANKLE_R, USE L_HIP_Y, USE L_HIP_R, USE L_HIP_P, USE L_KNEE_P, USE L_ANKLE_P, USE L_ANKLE_R, USE CHEST_P, USE NECK_Y, USE R_SHOULDER_P, USE R_SHOULDER_R, USE R_ELBOW_P, USE L_SHOULDER_P, USE L_SHOULDER_R, USE L_ELBOW_P, ] segments [ USE WAIST_LINK, USE R_HIP_Y_LINK, USE R_HIP_R_LINK, USE R_HIP_P_LINK, USE R_KNEE_P_LINK, USE R_ANKLE_P_LINK, USE R_ANKLE_R_LINK, USE L_HIP_Y_LINK, USE L_HIP_R_LINK, USE L_HIP_P_LINK, USE L_KNEE_P_LINK, USE L_ANKLE_P_LINK, USE L_ANKLE_R_LINK, USE CHEST_P_LINK, USE NECK_Y_LINK, USE R_SHOULDER_P_LINK, USE R_SHOULDER_R_LINK, USE R_ELBOW_P_LINK, USE L_SHOULDER_P_LINK, USE L_SHOULDER_R_LINK, USE L_ELBOW_P_LINK, ] } choreonoid-1.1.0+dfsg/share/models/GR001/GR001.yaml000066400000000000000000000037321207742442300213260ustar00rootroot00000000000000 modelFile: "GR001.wrl" standardPose: [ 0, 0, 20, -40, -20, 0, 0, 0, -20, 40, 20, 0, 0, 0, 20, 0, -20, -20, 0, 20 ] linkGroup: - name: UPPER-BODY links: - name: NECK links: [ NECK_Y ] - name: ARMS links: - name: R-ARM links: [ R_SHOULDER_P, R_SHOULDER_R, R_ELBOW_P ] - name: L-ARM links: [ L_SHOULDER_P, L_SHOULDER_R, L_ELBOW_P ] - name: CHEST links: [ CHEST_P ] - WAIST - name: LOWER-BODY links: - name: LEGS links: - name: R-LEG links: [ R_HIP_Y, R_HIP_R, R_HIP_P, R_KNEE_P, R_ANKLE_P, R_ANKLE_R ] - name: L-LEG links: [ L_HIP_Y, L_HIP_R, L_HIP_P, L_KNEE_P, L_ANKLE_P, L_ANKLE_R ] possibleIkInterpolationLinks: [ WAIST, R_ANKLE_R, L_ANKLE_R ] defaultIkInterpolationLinks: [ WAIST, R_ANKLE_R, L_ANKLE_R ] possileSupportLinks: [ R_ANKLE_R, L_ANKLE_R ] defaultIKsetup: WAIST: [ R_ANKLE_R, L_ANKLE_R ] R_ANKLE_R: [ WAIST ] L_ANKLE_R: [ WAIST ] footLinks: - link: R_ANKLE_R soleCenter: [ -0.005, -0.01, -0.022 ] - link: L_ANKLE_R soleCenter: [ -0.005, 0.01, -0.022 ] symmetricJoints: - [ NECK_Y ] - [ L_SHOULDER_P, R_SHOULDER_P, -1 ] - [ L_SHOULDER_R, R_SHOULDER_R, -1 ] - [ L_ELBOW_P, R_ELBOW_P, -1 ] - [ L_HIP_Y, R_HIP_Y, -1 ] - [ L_HIP_R, R_HIP_R, -1 ] - [ L_HIP_P, R_HIP_P, -1 ] - [ L_KNEE_P, R_KNEE_P, -1 ] - [ L_ANKLE_P, R_ANKLE_P, -1 ] - [ L_ANKLE_R, R_ANKLE_R, -1 ] symmetricIkLinks: - [ WAIST ] - [ L_ANKLE_R, R_ANKLE_R ] divisionNumberOfPrimitiveGeometries: 10 selfCollisionDetection: excludeTreeDepth: 3 excludeLinks: [ ] poseConversionInfo: - targetBodies: [ HRP4C ] jointMap: [ 0, 1, 2, 3, 4, 5, 7, 8, 9, 10, 11, 12, 12, -1, -1, 13, -1, -1, # faces 14, 15, -1, 16, -1, -1, -1, -1, -1, 17, 18, -1, 19, -1, -1, -1, -1, -1 ] linkMap: { R_FOOT: R_ANKLE_R, L_FOOT: L_ANKLE_R } choreonoid-1.1.0+dfsg/share/models/GR001/parts/000077500000000000000000000000001207742442300210355ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/models/GR001/parts/CHEST_P.wrl000066400000000000000000004347771207742442300227360ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 70.000 88.000 74.000 center 0.000 0.000 0.000 translation 0.000 -0.000 -0.00 #translation 21.000 -0.000 -28.000 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.30 0.30 0.70 emissiveColor 0.00 0.00 0.00 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -36.000 32.500 65.000, -36.000 25.500 65.000, -1.000 25.500 65.000, -1.000 32.500 65.000, 6.000 25.500 55.000, 6.000 25.500 35.605, 7.000 25.500 36.206, 7.000 25.500 57.000, -1.000 25.500 65.000, -41.000 25.500 55.000, -36.000 25.500 65.000, -41.000 25.500 60.000, 7.000 25.500 57.000, 7.000 32.500 57.000, -3.090 32.500 30.143, -3.004 32.500 30.000, -5.000 32.500 30.000, 1.000 32.500 36.000, 7.000 32.500 57.000, 7.000 32.500 36.206, 1.000 32.500 55.300, -4.700 32.500 61.000, -1.000 32.500 65.000, -37.172 32.500 61.000, -36.000 32.500 65.000, -41.000 32.500 57.172, -41.000 32.500 60.000, -41.000 32.500 60.000, -41.000 25.500 60.000, -41.000 32.500 57.172, -41.000 25.500 55.000, -41.000 32.500 60.000, -41.000 29.000 48.750, -41.000 32.500 37.500, -41.000 25.500 37.500, 7.000 25.500 36.206, 7.000 32.500 36.206, -41.000 -25.000 55.000, -42.000 25.500 55.000, -42.000 -25.000 55.000, -12.259 1.466 55.000, -12.500 1.366 55.000, 13.000 19.415 55.000, -11.034 0.759 55.000, -11.000 0.500 55.000, -12.000 1.500 55.000, -11.134 1.000 55.000, -11.293 1.207 55.000, 6.000 -25.000 55.000, -12.000 -0.500 55.000, -12.259 -0.466 55.000, -11.741 -0.466 55.000, -11.500 -0.366 55.000, -11.293 -0.207 55.000, -11.741 1.466 55.000, 13.000 -18.915 55.000, -11.134 0.000 55.000, -11.034 0.241 55.000, -11.500 1.366 55.000, -14.500 0.250 55.000, -12.707 1.207 55.000, -12.866 1.000 55.000, -12.966 0.759 55.000, -13.000 0.500 55.000, -12.966 0.241 55.000, -12.866 0.000 55.000, -12.707 -0.207 55.000, -12.500 -0.366 55.000, 7.000 32.500 36.206, 7.000 25.500 36.206, 6.000 25.500 35.605, 1.955 29.000 33.174, -3.090 32.500 30.143, -3.090 25.500 30.143, 13.000 19.415 30.000, 6.000 25.500 30.000, 9.500 22.457 42.500, 13.000 19.415 55.000, 6.000 25.500 55.000, -40.086 36.500 58.086, -37.172 32.500 61.000, -37.172 40.500 61.000, -43.000 40.500 55.172, -41.000 32.500 57.172, -43.000 32.500 55.172, -4.700 40.500 61.000, -4.700 32.500 61.000, 1.000 32.500 55.300, 1.000 40.500 55.300, 1.000 32.500 36.000, 1.000 40.500 36.000, -5.000 32.500 30.000, -5.000 40.500 30.000, -3.004 32.500 30.000, -3.004 25.500 30.000, -5.000 32.500 30.000, -18.252 29.000 30.000, -33.500 25.500 30.000, -33.500 32.500 30.000, -41.000 32.500 37.500, -35.515 32.500 30.000, -33.500 32.500 30.000, -43.000 32.500 37.485, -42.000 25.500 30.000, -42.000 25.500 55.000, -12.500 -0.366 57.000, -11.741 1.466 57.000, -11.500 1.366 57.000, -12.259 -0.466 57.000, -12.000 1.500 57.000, -12.000 -0.500 57.000, -12.259 1.466 57.000, -11.741 -0.466 57.000, -12.500 1.366 57.000, -11.500 -0.366 57.000, -12.707 1.207 57.000, -11.293 -0.207 57.000, -12.866 1.000 57.000, -11.134 0.000 57.000, -12.966 0.759 57.000, -11.034 0.241 57.000, -11.000 0.500 57.000, -13.000 0.500 57.000, -11.034 0.759 57.000, -12.966 0.241 57.000, -11.134 1.000 57.000, -12.866 0.000 57.000, -11.293 1.207 57.000, -12.707 -0.207 57.000, 13.000 16.500 39.000, 13.000 16.500 30.000, 13.000 -2.500 39.000, 13.000 -4.500 30.000, 13.000 -2.500 30.000, 13.000 -18.915 30.000, 13.000 -18.915 55.000, 13.000 0.250 42.500, 6.000 -25.000 55.000, 6.000 -25.000 45.833, 13.000 -18.915 55.000, 6.000 -25.000 30.000, 9.500 -21.957 42.500, 13.000 -18.915 30.000, 7.000 -25.000 36.206, 6.000 -25.000 35.605, 6.000 -25.000 45.833, 7.000 -25.000 57.000, -1.000 -25.000 65.000, -36.000 -25.000 65.000, -41.000 -25.000 60.000, -41.000 -25.000 37.500, -42.000 -25.000 30.000, -33.500 -25.000 30.000, -42.000 -25.000 55.000, -41.000 -25.000 55.000, -42.000 -25.000 58.000, -42.000 25.500 58.000, -3.004 -25.000 30.000, 9.000 -19.000 30.000, 6.000 -25.000 30.000, 6.000 -25.000 30.000, 9.000 -17.500 30.000, -9.000 19.500 30.000, 9.000 18.000 30.000, -5.000 16.500 30.000, -9.000 18.000 30.000, -9.000 -19.000 30.000, 9.000 19.500 30.000, -5.000 -4.500 30.000, -14.500 0.250 30.000, -42.000 -25.000 30.000, -33.500 -25.000 30.000, -9.000 -17.500 30.000, -4.700 40.500 61.000, -11.606 40.500 52.990, -13.500 40.500 52.858, 1.000 40.500 36.000, -4.036 40.500 45.753, 1.000 40.500 55.300, -4.437 40.500 47.609, -5.265 40.500 49.317, -15.394 40.500 52.990, -5.000 40.500 30.000, -6.763 40.500 38.952, -5.486 40.500 40.356, -17.266 40.500 52.672, -8.336 40.500 37.888, -6.472 40.500 50.783, -4.575 40.500 42.022, -10.115 40.500 37.225, -4.082 40.500 43.855, -7.990 40.500 51.923, -12.000 40.500 37.000, -22.964 40.500 45.753, -22.918 40.500 43.855, -43.000 40.500 37.485, -22.425 40.500 42.022, -35.515 40.500 30.000, -12.503 40.500 37.016, -22.563 40.500 47.609, -43.000 40.500 55.172, -19.010 40.500 51.923, -37.172 40.500 61.000, -20.528 40.500 50.783, -21.735 40.500 49.317, -13.500 40.500 37.142, -13.003 40.500 37.063, -13.997 40.500 37.063, -21.514 40.500 40.356, -20.237 40.500 38.952, -18.664 40.500 37.888, -16.885 40.500 37.225, -14.497 40.500 37.016, -9.734 40.500 52.672, -15.000 40.500 37.000, -43.000 40.500 37.485, -35.515 40.500 30.000, -20.257 36.500 30.000, -42.000 25.500 27.000, -56.000 25.500 27.000, -56.000 25.500 58.000, -49.000 25.500 42.500, -16.184 -3.801 57.000, -16.632 -3.314 57.000, -15.685 -4.235 57.000, -17.354 -2.209 57.000, -17.619 -1.604 57.000, -17.023 -2.782 57.000, -15.142 -4.612 57.000, -14.561 -4.926 57.000, -17.816 -0.973 57.000, -17.943 -0.324 57.000, -13.948 -5.175 57.000, -13.312 -5.355 57.000, -12.660 -5.464 57.000, -11.340 -5.464 57.000, -12.000 -5.500 57.000, -17.998 0.335 57.000, -17.980 0.995 57.000, -17.726 2.291 57.000, -17.495 2.910 57.000, -17.889 1.650 57.000, -10.688 -5.355 57.000, -10.052 -5.175 57.000, -9.439 -4.926 57.000, -8.858 -4.612 57.000, -8.315 -4.235 57.000, -17.196 3.500 57.000, -16.835 4.053 57.000, -7.816 -3.801 57.000, -16.414 4.564 57.000, -15.941 5.025 57.000, -7.369 -3.314 57.000, -15.419 5.431 57.000, -6.977 -2.782 57.000, -14.856 5.777 57.000, -6.646 -2.209 57.000, -14.258 6.059 57.000, -6.381 -1.604 57.000, -13.633 6.274 57.000, -6.184 -0.973 57.000, -12.988 6.418 57.000, -6.057 -0.324 57.000, -12.331 6.491 57.000, -6.002 0.335 57.000, -11.669 6.491 57.000, -6.020 0.995 57.000, -11.012 6.418 57.000, -6.111 1.650 57.000, -10.367 6.274 57.000, -6.274 2.291 57.000, -9.742 6.059 57.000, -6.505 2.910 57.000, -9.144 5.777 57.000, -6.804 3.500 57.000, -8.581 5.431 57.000, -7.165 4.053 57.000, -8.059 5.025 57.000, -7.586 4.564 57.000, 13.000 12.500 34.000, 13.000 12.500 30.000, 13.000 4.500 34.000, 13.000 4.500 30.000, -0.172 12.500 30.000, -0.172 4.500 30.000, 6.000 -25.000 30.000, 6.000 -25.000 35.605, -1.000 -25.000 65.000, -36.000 -25.000 65.000, -36.000 -32.000 65.000, -1.000 -32.000 65.000, -41.000 -25.000 60.000, -41.000 -32.000 60.000, 7.000 -25.000 57.000, 7.000 -32.000 57.000, -41.000 -32.000 57.172, -41.000 -32.000 37.500, -41.000 -28.500 48.750, 7.000 -32.000 36.206, 7.000 -25.000 36.206, 7.000 -25.000 36.206, -3.090 -32.000 30.143, 1.955 -28.500 33.174, -3.090 -25.000 30.143, -33.500 -32.000 30.000, -56.000 -25.000 27.000, -42.000 -25.000 27.000, -56.000 -25.000 58.000, -49.000 -25.000 42.500, 1.455 -25.000 32.802, -3.004 -25.000 30.000, -5.000 -32.000 30.000, -3.004 -32.000 30.000, -18.252 -28.500 30.000, 9.000 -19.000 27.500, 5.611 -19.000 7.036, 7.036 -19.000 5.611, 9.000 -19.000 30.000, -4.000 -19.000 29.000, 9.000 -19.000 29.000, 9.000 -19.000 0.000, 8.774 -19.000 2.003, -8.774 -19.000 2.003, -8.109 -19.000 3.905, -9.000 -19.000 30.000, -9.000 -19.000 0.000, -0.000 -19.000 15.000, -4.000 -19.000 27.500, 3.905 -19.000 8.109, 2.003 -19.000 8.774, -0.000 -19.000 9.000, -2.003 -19.000 8.774, -3.905 -19.000 8.109, -5.611 -19.000 7.036, -7.036 -19.000 5.611, 8.109 -19.000 3.905, -9.000 -17.500 0.000, -9.000 -19.000 0.000, 9.000 -17.500 30.000, 9.000 -17.500 29.000, -4.000 -17.500 27.500, -9.000 -17.500 30.000, -4.000 -17.500 29.000, 8.774 -17.500 2.003, 8.109 -17.500 3.905, 9.000 -17.500 27.500, 7.036 -17.500 5.611, 5.611 -17.500 7.036, 9.000 -17.500 0.000, -8.109 -17.500 3.905, -8.774 -17.500 2.003, -9.000 -17.500 0.000, -0.000 -17.500 15.000, 3.905 -17.500 8.109, 2.003 -17.500 8.774, -0.000 -17.500 9.000, -2.003 -17.500 8.774, -3.905 -17.500 8.109, -5.611 -17.500 7.036, -7.036 -17.500 5.611, -4.000 18.000 29.000, -4.000 18.000 27.500, 9.000 18.000 27.500, 8.109 18.000 3.905, 8.774 18.000 2.003, 7.036 18.000 5.611, 5.611 18.000 7.036, 9.000 18.000 29.000, 9.000 18.000 0.000, -8.774 18.000 2.003, -8.109 18.000 3.905, -9.000 18.000 0.000, 0.000 18.000 15.000, 3.905 18.000 8.109, 2.003 18.000 8.774, -0.000 18.000 9.000, -2.003 18.000 8.774, -3.905 18.000 8.109, -5.611 18.000 7.036, -7.036 18.000 5.611, -9.000 19.500 0.000, 9.000 19.500 29.000, -4.000 19.500 27.500, -4.000 19.500 29.000, 8.774 19.500 2.003, 8.109 19.500 3.905, 9.000 19.500 27.500, 7.036 19.500 5.611, 5.611 19.500 7.036, 9.000 19.500 0.000, -8.109 19.500 3.905, -8.774 19.500 2.003, 0.000 19.500 15.000, 3.905 19.500 8.109, 2.003 19.500 8.774, -0.000 19.500 9.000, -2.003 19.500 8.774, -3.905 19.500 8.109, -5.611 19.500 7.036, -7.036 19.500 5.611, -19.789 40.500 43.173, -19.166 40.500 41.443, -18.164 40.500 39.901, -16.837 40.500 38.628, -15.255 40.500 37.692, -15.000 40.500 37.000, -20.528 40.500 50.783, -18.164 40.500 50.099, -14.497 40.500 37.016, -13.997 40.500 37.063, -16.837 40.500 51.372, -21.735 40.500 49.317, -19.166 40.500 48.557, -13.500 40.500 37.142, -15.394 40.500 52.990, -15.255 40.500 52.308, -19.789 40.500 46.827, -13.500 40.500 52.858, -20.000 40.500 45.000, -11.104 41.500 37.050, -12.000 40.500 37.000, -12.000 41.500 37.000, -10.115 40.500 37.225, -10.220 41.500 37.201, -9.358 41.500 37.449, -8.336 40.500 37.888, -8.529 41.500 37.792, -7.744 41.500 38.226, -6.763 40.500 38.952, -7.012 41.500 38.745, -6.343 41.500 39.343, -5.486 40.500 40.356, -5.745 41.500 40.012, -5.226 41.500 40.744, -4.792 41.500 41.529, -4.575 40.500 42.022, -4.449 41.500 42.358, -4.201 41.500 43.220, -4.082 40.500 43.855, -4.050 41.500 44.104, -4.000 41.500 45.000, -4.036 40.500 45.753, -4.050 41.500 45.896, -4.201 41.500 46.780, -4.437 40.500 47.609, -4.449 41.500 47.642, -4.792 41.500 48.471, -5.265 40.500 49.317, -5.226 41.500 49.256, -5.745 41.500 49.988, -6.472 40.500 50.783, -6.343 41.500 50.657, -7.012 41.500 51.255, -7.990 40.500 51.923, -7.744 41.500 51.774, -8.529 41.500 52.208, -9.734 40.500 52.672, -9.358 41.500 52.551, -10.220 41.500 52.799, -11.104 41.500 52.950, -11.606 40.500 52.990, -12.000 41.500 53.000, -12.896 41.500 52.950, -13.780 41.500 52.799, -14.642 41.500 52.551, -15.471 41.500 52.208, -16.256 41.500 51.774, -16.988 41.500 51.255, -17.657 41.500 50.657, -18.255 41.500 49.988, -18.774 41.500 49.256, -19.208 41.500 48.471, -19.551 41.500 47.642, -19.799 41.500 46.780, -19.950 41.500 45.896, -20.000 41.500 45.000, -19.950 41.500 44.104, -19.799 41.500 43.220, -19.551 41.500 42.358, -19.208 41.500 41.529, -18.774 41.500 40.744, -18.255 41.500 40.012, -17.657 41.500 39.343, -16.988 41.500 38.745, -16.256 41.500 38.226, -15.471 41.500 37.792, -14.642 41.500 37.449, -13.780 41.500 37.201, -12.896 41.500 37.050, -13.003 40.500 37.063, -12.503 40.500 37.016, -19.500 -7.000 57.000, -4.500 -7.000 57.000, -19.500 8.000 57.000, -4.500 8.000 57.000, -5.000 -32.000 30.000, -3.004 -32.000 30.000, -3.090 -32.000 30.143, 1.000 -32.000 36.000, 1.000 -32.000 55.300, 7.000 -32.000 36.206, 7.000 -32.000 57.000, -1.000 -32.000 65.000, -4.700 -32.000 61.000, -36.000 -32.000 65.000, -37.172 -32.000 61.000, -41.000 -32.000 60.000, -41.000 -32.000 57.172, -35.515 -32.000 30.000, -41.000 -32.000 37.500, -43.000 -32.000 37.485, -43.000 -32.000 55.172, -41.000 -32.000 57.172, -35.515 -40.000 30.000, -5.000 -40.000 30.000, -20.257 -36.000 30.000, -5.000 -32.000 30.000, -4.000 -27.500 27.500, -4.000 -27.500 29.000, 12.500 19.285 27.500, 12.500 18.000 27.500, 0.491 -27.500 27.500, -4.000 -16.000 27.500, 9.000 -16.000 27.500, 12.500 -18.285 27.500, 12.500 -17.000 27.500, 9.000 17.000 27.500, -4.000 17.000 27.500, 0.491 28.500 27.500, -4.000 28.500 27.500, -1.294 -19.000 4.830, -2.003 -19.000 8.774, -3.905 -19.000 8.109, -0.000 -19.000 5.000, -2.500 -19.000 4.330, -5.611 -19.000 7.036, -3.536 -19.000 3.536, -7.036 -19.000 5.611, -4.330 -19.000 2.500, -8.109 -19.000 3.905, -4.830 -19.000 1.294, 3.536 -19.000 3.536, 7.036 -19.000 5.611, 5.611 -19.000 7.036, -8.774 -19.000 2.003, -5.000 -19.000 0.000, 4.330 -19.000 2.500, 8.109 -19.000 3.905, 2.500 -19.000 4.330, 3.905 -19.000 8.109, 4.830 -19.000 1.294, 8.774 -19.000 2.003, 1.294 -19.000 4.830, 2.003 -19.000 8.774, 5.000 -19.000 0.000, 9.000 -19.000 0.000, -0.000 -19.000 9.000, 0.491 -27.500 29.000, -4.000 -16.000 29.000, 9.000 -16.000 29.000, 12.500 -18.285 29.000, 12.500 -17.000 29.000, 9.000 17.000 29.000, -4.000 17.000 29.000, -4.000 28.500 29.000, 0.491 28.500 29.000, 12.500 18.000 29.000, 12.500 19.285 29.000, 7.036 -19.000 -5.611, 5.611 -17.500 -7.036, 7.036 -17.500 -5.611, 5.611 -19.000 -7.036, -8.774 -17.500 -2.003, 8.109 -17.500 -3.905, 8.109 -19.000 -3.905, -8.774 -19.000 -2.003, 8.774 -17.500 -2.003, -8.109 -17.500 -3.905, 8.774 -19.000 -2.003, -8.109 -19.000 -3.905, 9.000 -17.500 0.000, -7.036 -17.500 -5.611, -7.036 -19.000 -5.611, -5.611 -19.000 -7.036, -5.611 -17.500 -7.036, -3.905 -17.500 -8.109, -3.905 -19.000 -8.109, -2.003 -19.000 -8.774, -2.003 -17.500 -8.774, -0.000 -19.000 -9.000, -0.000 -17.500 -9.000, 2.003 -17.500 -8.774, 2.003 -19.000 -8.774, 3.905 -19.000 -8.109, 3.905 -17.500 -8.109, -3.905 -17.500 8.109, -2.003 -17.500 8.774, -1.294 -17.500 4.830, -0.000 -17.500 5.000, -2.500 -17.500 4.330, -5.611 -17.500 7.036, -3.536 -17.500 3.536, -7.036 -17.500 5.611, -4.330 -17.500 2.500, -4.830 -17.500 1.294, -8.109 -17.500 3.905, 5.611 -17.500 7.036, 7.036 -17.500 5.611, 3.536 -17.500 3.536, -5.000 -17.500 0.000, -8.774 -17.500 2.003, 8.109 -17.500 3.905, 4.330 -17.500 2.500, 3.905 -17.500 8.109, 2.500 -17.500 4.330, 8.774 -17.500 2.003, 4.830 -17.500 1.294, 2.003 -17.500 8.774, 1.294 -17.500 4.830, 5.000 -17.500 0.000, -0.000 -17.500 9.000, -1.294 18.000 4.830, -0.000 18.000 5.000, -2.500 18.000 4.330, -5.611 18.000 7.036, -3.536 18.000 3.536, -7.036 18.000 5.611, -4.330 18.000 2.500, -8.109 18.000 3.905, -4.830 18.000 1.294, 3.536 18.000 3.536, 7.036 18.000 5.611, 5.611 18.000 7.036, -8.774 18.000 2.003, -5.000 18.000 0.000, 4.330 18.000 2.500, 8.109 18.000 3.905, 2.500 18.000 4.330, 3.905 18.000 8.109, 4.830 18.000 1.294, 8.774 18.000 2.003, 1.294 18.000 4.830, 5.000 18.000 0.000, -0.000 18.000 9.000, 7.036 18.000 -5.611, 5.611 19.500 -7.036, 7.036 19.500 -5.611, 5.611 18.000 -7.036, -8.774 19.500 -2.003, 8.109 19.500 -3.905, 8.109 18.000 -3.905, -8.774 18.000 -2.003, 8.774 19.500 -2.003, -8.109 19.500 -3.905, 8.774 18.000 -2.003, -8.109 18.000 -3.905, -7.036 19.500 -5.611, -7.036 18.000 -5.611, -5.611 18.000 -7.036, -5.611 19.500 -7.036, -3.905 19.500 -8.109, -3.905 18.000 -8.109, -2.003 18.000 -8.774, -2.003 19.500 -8.774, -0.000 18.000 -9.000, -0.000 19.500 -9.000, 2.003 19.500 -8.774, 2.003 18.000 -8.774, 3.905 18.000 -8.109, 3.905 19.500 -8.109, -1.294 19.500 4.830, -0.000 19.500 5.000, -2.500 19.500 4.330, -5.611 19.500 7.036, -3.536 19.500 3.536, -7.036 19.500 5.611, -4.330 19.500 2.500, -4.830 19.500 1.294, -8.109 19.500 3.905, 5.611 19.500 7.036, 7.036 19.500 5.611, 3.536 19.500 3.536, -5.000 19.500 0.000, -8.774 19.500 2.003, 8.109 19.500 3.905, 4.330 19.500 2.500, 3.905 19.500 8.109, 2.500 19.500 4.330, 8.774 19.500 2.003, 4.830 19.500 1.294, 1.294 19.500 4.830, 5.000 19.500 0.000, -0.000 19.500 9.000, -11.120 42.500 37.049, -12.000 42.500 37.000, -10.251 42.500 37.194, -9.402 42.500 37.433, -8.586 42.500 37.765, -7.811 42.500 38.185, -7.086 42.500 38.687, -6.422 42.500 39.266, -5.825 42.500 39.914, -5.303 42.500 40.624, -4.862 42.500 41.388, -4.508 42.500 42.195, -4.245 42.500 43.036, -4.076 42.500 43.901, -4.003 42.500 44.780, -4.027 42.500 45.661, -4.148 42.500 46.534, -4.365 42.500 47.388, -4.674 42.500 48.214, -5.072 42.500 49.000, -5.554 42.500 49.738, -6.114 42.500 50.418, -6.746 42.500 51.033, -7.442 42.500 51.574, -8.192 42.500 52.036, -8.990 42.500 52.412, -9.823 42.500 52.698, -10.683 42.500 52.891, -11.559 42.500 52.988, -12.441 42.500 52.988, -13.317 42.500 52.891, -14.177 42.500 52.698, -15.010 42.500 52.412, -15.808 42.500 52.036, -16.558 42.500 51.574, -17.254 42.500 51.033, -17.886 42.500 50.418, -18.446 42.500 49.738, -18.928 42.500 49.000, -19.326 42.500 48.214, -19.635 42.500 47.388, -19.852 42.500 46.534, -19.973 42.500 45.661, -19.997 42.500 44.780, -19.924 42.500 43.901, -19.755 42.500 43.036, -19.492 42.500 42.195, -19.138 42.500 41.388, -18.697 42.500 40.624, -18.175 42.500 39.914, -17.578 42.500 39.266, -16.914 42.500 38.687, -16.189 42.500 38.185, -15.414 42.500 37.765, -14.598 42.500 37.433, -13.749 42.500 37.194, -12.880 42.500 37.049, -24.500 8.000 57.000, -19.500 8.000 57.000, -19.500 -7.000 57.000, -24.500 -7.000 57.000, -19.500 -12.000 57.000, -4.500 -7.000 57.000, -4.500 -12.000 57.000, -19.500 13.000 57.000, -4.500 8.000 57.000, -4.500 13.000 57.000, 0.500 8.000 57.000, 0.500 -7.000 57.000, -37.172 -40.000 61.000, -37.172 -32.000 61.000, -40.086 -36.000 58.086, -43.000 -40.000 55.172, -4.700 -40.000 61.000, -4.700 -32.000 61.000, 1.000 -40.000 55.300, 1.000 -32.000 55.300, 1.000 -32.000 36.000, 1.000 -40.000 36.000, -43.000 -40.000 37.485, -8.825 -40.000 39.914, -5.000 -40.000 30.000, 1.000 -40.000 36.000, -9.422 -40.000 39.266, -10.086 -40.000 38.687, -8.303 -40.000 40.624, -10.811 -40.000 38.185, -7.862 -40.000 41.388, -11.586 -40.000 37.765, -7.508 -40.000 42.195, -12.402 -40.000 37.433, -7.245 -40.000 43.036, -13.251 -40.000 37.194, -7.076 -40.000 43.901, -14.120 -40.000 37.049, -7.003 -40.000 44.780, -15.000 -40.000 37.000, -7.027 -40.000 45.661, -15.880 -40.000 37.049, -16.749 -40.000 37.194, 1.000 -40.000 55.300, -7.148 -40.000 46.534, -7.365 -40.000 47.388, -7.674 -40.000 48.214, -8.072 -40.000 49.000, -8.554 -40.000 49.738, -9.114 -40.000 50.418, -35.515 -40.000 30.000, -22.138 -40.000 41.388, -22.492 -40.000 42.195, -21.697 -40.000 40.624, -21.175 -40.000 39.914, -20.578 -40.000 39.266, -19.914 -40.000 38.687, -19.189 -40.000 38.185, -18.414 -40.000 37.765, -17.598 -40.000 37.433, -4.700 -40.000 61.000, -9.746 -40.000 51.033, -10.442 -40.000 51.574, -11.192 -40.000 52.036, -11.990 -40.000 52.412, -12.823 -40.000 52.698, -22.755 -40.000 43.036, -13.683 -40.000 52.891, -14.559 -40.000 52.988, -15.441 -40.000 52.988, -16.317 -40.000 52.891, -43.000 -40.000 37.485, -22.997 -40.000 44.780, -22.973 -40.000 45.661, -22.924 -40.000 43.901, -43.000 -40.000 55.172, -22.852 -40.000 46.534, -22.635 -40.000 47.388, -37.172 -40.000 61.000, -17.177 -40.000 52.698, -18.010 -40.000 52.412, -18.808 -40.000 52.036, -19.558 -40.000 51.574, -20.254 -40.000 51.033, -20.886 -40.000 50.418, -21.446 -40.000 49.738, -21.928 -40.000 49.000, -22.326 -40.000 48.214, -4.000 -29.000 29.000, -4.000 -27.500 27.500, -4.000 -29.000 27.500, 0.491 -27.500 4.000, 0.491 -27.500 27.500, -4.000 -27.500 4.000, 6.495 -22.893 15.750, 12.500 -18.285 13.000, 12.500 -18.285 4.000, 1.000 -27.109 4.000, 12.500 -18.285 27.500, 12.500 -18.000 14.575, 12.500 -18.000 13.000, 12.500 -17.643 20.250, 12.500 -17.000 27.500, 12.500 -17.000 15.767, 12.500 -14.572 18.660, 12.500 -12.795 13.000, 12.500 -10.742 15.446, 12.500 11.742 15.446, 12.500 13.795 13.000, 12.500 15.572 18.660, 12.500 18.000 27.500, 12.500 18.000 15.767, 12.500 0.500 20.250, 12.500 19.000 13.000, 12.500 19.000 14.575, 12.500 19.285 13.000, 12.500 19.285 27.500, 12.500 18.643 20.250, 0.491 28.500 27.500, 0.491 28.500 4.000, 6.495 23.893 15.750, 1.000 28.109 4.000, 12.500 19.285 4.000, -4.000 28.500 27.500, -4.000 28.500 4.000, 9.000 17.000 27.500, 9.000 17.000 29.000, -4.000 17.000 27.500, -4.000 17.000 29.000, 9.000 -16.000 27.500, 9.000 -16.000 29.000, -4.000 -16.000 27.500, -4.000 -16.000 29.000, -0.000 -19.000 -5.000, -4.830 -19.000 -1.294, -1.294 -19.000 -4.830, 4.830 -19.000 -1.294, -4.330 -19.000 -2.500, 4.330 -19.000 -2.500, -2.500 -19.000 -4.330, -3.536 -19.000 -3.536, 3.536 -19.000 -3.536, 2.500 -19.000 -4.330, 1.294 -19.000 -4.830, -3.536 -17.500 -3.536, -2.500 -17.500 -4.330, 1.294 -17.500 -4.830, -0.000 -17.500 -5.000, -1.294 -17.500 -4.830, 2.500 -17.500 -4.330, 3.536 -17.500 -3.536, 4.330 -17.500 -2.500, 4.830 -17.500 -1.294, -4.830 -17.500 -1.294, -4.330 -17.500 -2.500, 1.000 -29.000 29.000, -1.500 -28.250 29.000, 0.087 -27.810 29.000, 0.491 -27.500 29.000, 13.894 -19.106 29.000, 13.928 -19.072 29.000, 13.872 -19.000 29.000, 12.500 -19.000 29.000, 1.000 -29.000 29.000, 7.209 -23.643 29.000, 1.000 -27.500 29.000, 13.872 -19.000 29.000, 14.000 -19.000 29.000, 14.000 -19.000 29.000, 12.500 -18.285 29.000, 13.019 -17.887 29.000, 12.500 -17.000 29.000, 14.000 -17.000 29.000, 13.250 -18.000 29.000, 14.000 18.000 29.000, 12.500 18.000 29.000, 12.500 19.285 29.000, 13.019 18.887 29.000, 13.872 20.000 29.000, 14.000 20.000 29.000, 14.000 20.000 29.000, 13.250 19.000 29.000, 12.500 20.000 29.000, 13.872 20.000 29.000, 13.894 20.106 29.000, 13.928 20.072 29.000, 7.209 24.643 29.000, 1.000 28.500 29.000, 1.000 30.000 29.000, -4.000 28.500 29.000, 0.491 28.500 29.000, -1.500 29.250 29.000, -4.000 30.000 29.000, 0.087 28.810 29.000, 1.000 30.000 29.000, 0.000 18.000 -5.000, -4.830 18.000 -1.294, 4.830 18.000 -1.294, -1.294 18.000 -4.830, -4.330 18.000 -2.500, 4.330 18.000 -2.500, -2.500 18.000 -4.330, 3.536 18.000 -3.536, -3.536 18.000 -3.536, 2.500 18.000 -4.330, 1.294 18.000 -4.830, -3.536 19.500 -3.536, -2.500 19.500 -4.330, 1.294 19.500 -4.830, 0.000 19.500 -5.000, -1.294 19.500 -4.830, 2.500 19.500 -4.330, 3.536 19.500 -3.536, 4.330 19.500 -2.500, 4.830 19.500 -1.294, -4.830 19.500 -1.294, -4.330 19.500 -2.500, -4.000 30.000 27.500, -17.886 42.500 50.418, -13.352 42.500 46.473, -13.511 42.500 46.310, -18.446 42.500 49.738, -13.176 42.500 46.618, -13.651 42.500 46.129, -18.928 42.500 49.000, -12.983 42.500 46.741, -15.808 42.500 52.036, -13.769 42.500 45.933, -19.326 42.500 48.214, -12.779 42.500 46.842, -15.010 42.500 52.412, -13.864 42.500 45.726, -12.563 42.500 46.919, -14.177 42.500 52.698, -13.934 42.500 45.508, -19.852 42.500 46.534, -12.341 42.500 46.971, -13.317 42.500 52.891, -13.980 42.500 45.285, -12.441 42.500 52.988, -11.559 42.500 52.988, -12.114 42.500 46.997, -13.999 42.500 45.057, -11.886 42.500 46.997, -10.683 42.500 52.891, -13.993 42.500 44.829, -9.823 42.500 52.698, -11.659 42.500 46.971, -19.755 42.500 43.036, -13.960 42.500 44.603, -8.990 42.500 52.412, -11.437 42.500 46.919, -13.902 42.500 44.382, -11.221 42.500 46.842, -8.192 42.500 52.036, -13.819 42.500 44.169, -11.017 42.500 46.741, -7.442 42.500 51.574, -13.713 42.500 43.967, -10.824 42.500 46.618, -6.746 42.500 51.033, -13.584 42.500 43.779, -10.648 42.500 46.473, -13.434 42.500 43.606, -13.266 42.500 43.452, -6.114 42.500 50.418, -10.489 42.500 46.310, -16.914 42.500 38.687, -13.081 42.500 43.317, -10.349 42.500 46.129, -5.554 42.500 49.738, -12.882 42.500 43.205, -5.072 42.500 49.000, -10.231 42.500 45.933, -15.414 42.500 37.765, -12.672 42.500 43.116, -10.136 42.500 45.726, -4.674 42.500 48.214, -14.598 42.500 37.433, -12.453 42.500 43.052, -10.066 42.500 45.508, -4.365 42.500 47.388, -12.228 42.500 43.013, -13.749 42.500 37.194, -10.020 42.500 45.285, -4.148 42.500 46.534, -12.000 42.500 43.000, -12.880 42.500 37.049, -10.001 42.500 45.057, -4.027 42.500 45.661, -12.000 42.500 37.000, -10.007 42.500 44.829, -4.003 42.500 44.780, -11.772 42.500 43.013, -11.120 42.500 37.049, -4.076 42.500 43.901, -10.040 42.500 44.603, -11.547 42.500 43.052, -10.251 42.500 37.194, -10.098 42.500 44.382, -4.245 42.500 43.036, -11.328 42.500 43.116, -9.402 42.500 37.433, -10.181 42.500 44.169, -4.508 42.500 42.195, -11.118 42.500 43.205, -8.586 42.500 37.765, -10.287 42.500 43.967, -4.862 42.500 41.388, -10.919 42.500 43.317, -7.811 42.500 38.185, -10.416 42.500 43.779, -5.303 42.500 40.624, -10.734 42.500 43.452, -7.086 42.500 38.687, -10.566 42.500 43.606, -5.825 42.500 39.914, -6.422 42.500 39.266, -19.500 8.000 58.000, -19.500 8.000 57.000, -24.500 8.000 58.000, -24.500 -7.000 57.000, -19.500 -7.000 57.000, -19.500 -7.000 58.000, -24.500 -7.000 58.000, -19.500 -12.000 57.000, -19.500 -12.000 58.000, -4.500 -12.000 57.000, -4.500 -7.000 57.000, -4.500 -7.000 58.000, -4.500 -12.000 58.000, -19.500 13.000 58.000, -19.500 13.000 57.000, -4.500 13.000 57.000, -4.500 13.000 58.000, -4.500 8.000 58.000, 0.500 8.000 58.000, 0.500 8.000 57.000, 0.500 -7.000 58.000, 0.500 -7.000 57.000, -16.414 -40.000 46.414, -16.000 -40.000 46.732, -16.732 -40.000 46.000, -15.518 -40.000 46.932, -16.932 -40.000 45.518, -15.000 -40.000 47.000, -17.000 -40.000 45.000, -14.482 -40.000 46.932, -16.932 -40.000 44.482, -14.000 -40.000 46.732, -16.732 -40.000 44.000, -16.414 -40.000 43.586, -16.000 -40.000 43.268, -12.707 -40.000 45.707, -13.268 -40.000 46.000, -13.068 -40.000 45.518, -12.500 -40.000 45.866, -13.586 -40.000 46.414, -12.866 -40.000 45.500, -12.259 -40.000 45.966, -12.966 -40.000 45.259, -13.000 -40.000 45.000, -12.000 -40.000 46.000, -11.741 -40.000 45.966, -13.068 -40.000 44.482, -12.966 -40.000 44.741, -11.500 -40.000 45.866, -12.866 -40.000 44.500, -11.293 -40.000 45.707, -12.707 -40.000 44.293, -13.268 -40.000 44.000, -12.500 -40.000 44.134, -13.586 -40.000 43.586, -12.259 -40.000 44.034, -15.518 -40.000 43.068, -12.000 -40.000 44.000, -14.000 -40.000 43.268, -11.134 -40.000 45.500, -11.034 -40.000 45.259, -15.000 -40.000 43.000, -11.000 -40.000 45.000, -11.034 -40.000 44.741, -14.482 -40.000 43.068, -11.134 -40.000 44.500, -11.293 -40.000 44.293, -11.500 -40.000 44.134, -11.741 -40.000 44.034, -4.000 -29.000 4.000, 1.000 -29.000 27.500, -1.500 -28.250 4.000, 1.000 -29.000 4.000, 0.087 -27.810 4.000, 12.500 -18.000 8.483, 12.500 -18.285 -4.000, 12.500 -17.000 -4.000, 12.500 -17.170 7.786, 12.500 -17.643 4.500, 12.500 -17.000 7.988, 7.703 -21.966 0.367, 6.922 -22.565 1.378, 5.973 -23.293 2.266, 8.302 -21.506 -0.733, 4.874 -24.136 2.999, 8.718 -21.187 -1.893, 8.929 -21.025 -2.937, 3.654 -25.073 3.547, 9.000 -20.971 -4.000, 2.346 -26.076 3.886, 0.492 -27.499 3.984, 0.746 -27.304 3.996, 0.087 -27.810 3.948, 12.500 -17.000 13.000, 12.500 15.552 14.795, 12.500 18.000 13.000, 14.000 -12.795 13.000, 14.000 13.795 13.000, 12.500 -14.552 14.795, 12.500 19.000 8.483, 12.500 18.000 -4.000, 12.500 19.285 -4.000, 12.500 18.000 7.988, 12.500 18.643 4.500, 12.500 18.170 7.786, 0.593 28.421 3.990, 0.340 28.616 3.973, 0.087 28.810 4.000, 0.087 28.810 3.948, 7.233 23.326 1.015, 8.003 22.736 -0.132, 6.257 24.075 2.030, 8.556 22.311 -1.372, 5.094 24.968 2.873, 8.888 22.056 -2.667, 3.773 25.981 3.504, 9.000 21.971 -4.000, 2.409 27.028 3.875, -4.000 30.000 4.000, 1.000 30.000 4.000, -1.500 29.250 4.000, 13.894 -19.106 29.000, 13.932 -19.077 29.000, 13.928 -19.072 29.000, 12.500 -19.000 29.000, 13.186 -18.444 29.000, 14.000 -19.000 29.000, 14.000 -19.000 27.500, 14.000 -17.000 27.500, 14.000 18.000 27.500, 14.000 20.000 27.500, 14.000 20.000 29.000, 13.928 20.072 29.000, 12.500 20.000 29.000, 13.932 20.077 29.000, 13.894 20.106 29.000, 1.000 30.000 27.500, -12.707 42.500 45.707, -12.500 42.500 45.866, -12.866 42.500 45.500, -12.259 42.500 45.966, -12.966 42.500 45.259, -12.000 42.500 46.000, -13.000 42.500 45.000, -11.741 42.500 45.966, -12.966 42.500 44.741, -11.500 42.500 45.866, -12.866 42.500 44.500, -11.293 42.500 45.707, -12.707 42.500 44.293, -12.500 42.500 44.134, -11.134 42.500 45.500, -11.034 42.500 45.259, -12.259 42.500 44.034, -11.000 42.500 45.000, -12.000 42.500 44.000, -11.034 42.500 44.741, -11.741 42.500 44.034, -11.500 42.500 44.134, -11.134 42.500 44.500, -11.293 42.500 44.293, -19.500 8.000 59.000, -24.500 8.000 59.000, -19.500 -7.000 59.000, -24.500 -7.000 59.000, -19.500 -12.000 59.000, -4.500 -7.000 59.000, -4.500 -12.000 59.000, -19.500 13.000 59.000, -4.500 13.000 59.000, -4.500 8.000 59.000, 0.500 8.000 59.000, 0.500 -7.000 59.000, -16.732 -40.500 46.000, -16.932 -40.500 45.518, -13.058 -40.500 45.477, -13.000 -40.500 45.000, -17.000 -40.500 45.000, -16.932 -40.500 44.482, -13.228 -40.500 45.927, -16.732 -40.500 44.000, -13.500 -40.500 46.323, -13.929 -40.500 46.689, -16.414 -40.500 43.586, -14.442 -40.500 46.921, -16.000 -40.500 43.268, -15.518 -40.500 43.068, -15.000 -40.500 47.000, -15.000 -40.500 43.000, -15.518 -40.500 46.932, -16.000 -40.500 46.732, -16.414 -40.500 46.414, -14.442 -40.500 43.079, -13.929 -40.500 43.311, -13.500 -40.500 43.677, -13.228 -40.500 44.073, -13.058 -40.500 44.523, -11.134 -40.500 45.500, -11.034 -40.500 45.259, -12.966 -40.500 45.259, -11.000 -40.500 45.000, -12.866 -40.500 45.500, -11.034 -40.500 44.741, -12.707 -40.500 45.707, -11.134 -40.500 44.500, -12.500 -40.500 45.866, -11.293 -40.500 44.293, -12.259 -40.500 45.966, -11.500 -40.500 44.134, -12.000 -40.500 46.000, -11.741 -40.500 44.034, -11.741 -40.500 45.966, -12.000 -40.500 44.000, -11.500 -40.500 45.866, -11.293 -40.500 45.707, -12.259 -40.500 44.034, -12.500 -40.500 44.134, -12.707 -40.500 44.293, -12.866 -40.500 44.500, -12.966 -40.500 44.741, 7.466 -24.039 16.500, 13.932 -19.077 4.000, 0.492 -28.338 3.984, 0.746 -28.669 3.996, 12.500 -17.378 10.639, 14.000 -17.000 7.988, 14.000 -17.000 -4.000, 13.019 -17.887 -4.000, 13.250 -18.000 -4.000, 14.000 -19.000 -4.000, 13.872 -19.000 -4.000, 1.000 -27.500 4.000, 2.372 -27.947 3.881, 3.704 -26.925 3.529, 4.942 -25.975 2.961, 6.053 -25.123 2.202, 7.006 -24.391 1.284, 7.783 -23.796 0.242, 8.370 -23.345 -0.889, 8.766 -23.041 -2.078, 8.941 -22.906 -3.032, 9.000 -22.861 -4.000, 13.932 -19.077 -4.000, 12.500 -19.000 -4.000, 11.009 -20.374 -4.000, 14.000 -17.000 15.767, 14.000 -14.572 18.660, 14.000 11.742 15.446, 14.000 -10.742 15.446, 14.000 18.000 15.767, 14.000 15.572 18.660, 14.000 0.500 20.250, 14.000 18.000 7.988, 13.019 18.887 -4.000, 14.000 18.000 -4.000, 13.250 19.000 -4.000, 14.000 20.000 -4.000, 13.872 20.000 -4.000, 12.500 18.378 10.639, 0.340 29.140 3.973, 0.593 29.470 3.990, 1.000 28.500 4.000, 2.311 28.994 3.892, 3.586 28.016 3.571, 4.948 26.970 2.958, 6.153 26.046 2.119, 7.165 25.269 1.098, 7.964 24.656 -0.063, 8.539 24.215 -1.323, 8.884 23.951 -2.642, 9.000 23.861 -4.000, 12.500 20.000 -4.000, 13.932 20.077 -4.000, 11.009 21.374 -4.000, 13.928 -19.072 27.500, 13.872 -19.000 4.000, 13.872 -19.000 13.000, 13.902 -19.039 16.500, 13.872 -19.000 27.500, 14.000 -19.000 27.500, 14.000 -19.000 13.000, 13.936 -19.000 21.000, 14.000 -18.000 13.000, 14.000 -18.000 14.575, 14.000 -18.000 21.000, 14.000 20.000 13.000, 14.000 19.000 14.575, 14.000 19.000 13.000, 14.000 19.000 21.000, 14.000 20.000 27.500, 13.936 20.000 21.000, 13.872 20.000 13.000, 13.872 20.000 27.500, 13.928 20.072 27.500, 7.466 25.039 16.500, 13.932 20.077 4.000, 13.872 20.000 4.000, 13.902 20.039 16.500, -12.249 44.000 44.031, -12.000 44.000 44.000, -12.482 44.000 44.124, -12.685 44.000 44.271, -12.844 44.000 44.464, -12.951 44.000 44.691, -12.998 44.000 44.937, -12.982 44.000 45.187, -12.905 44.000 45.426, -12.771 44.000 45.637, -12.588 44.000 45.809, -12.368 44.000 45.930, -12.125 44.000 45.992, -11.875 44.000 45.992, -11.632 44.000 45.930, -11.412 44.000 45.809, -11.229 44.000 45.637, -11.095 44.000 45.426, -11.018 44.000 45.187, -11.002 44.000 44.937, -11.049 44.000 44.691, -11.156 44.000 44.464, -11.315 44.000 44.271, -11.518 44.000 44.124, -11.751 44.000 44.031, -19.500 -7.000 59.000, -19.500 8.000 59.000, -24.500 -7.000 59.000, -4.500 -7.000 59.000, -19.500 -12.000 59.000, -4.500 -12.000 59.000, -19.500 13.000 59.000, -4.500 13.000 59.000, 0.500 -7.000 59.000, 0.500 8.000 59.000, -12.558 -40.500 43.079, -11.073 -40.500 46.772, -13.071 -40.500 43.311, -13.100 -40.500 46.671, -10.640 -40.500 46.466, -10.084 -40.500 45.574, -10.001 -40.500 45.050, -12.622 -40.500 46.901, -12.000 -40.500 43.000, -10.058 -40.500 44.523, -12.100 -40.500 46.997, -11.474 -40.500 43.070, -10.302 -40.500 46.057, -11.572 -40.500 46.954, -10.251 -40.500 44.029, -10.986 -40.500 43.276, -10.568 -40.500 43.604, -19.326 -40.500 48.214, -18.928 -40.500 49.000, -18.446 -40.500 49.738, -17.886 -40.500 50.418, -19.635 -40.500 47.388, -19.852 -40.500 46.534, -17.254 -40.500 51.033, -19.973 -40.500 45.661, -16.558 -40.500 51.574, -15.808 -40.500 52.036, -19.997 -40.500 44.780, -19.924 -40.500 43.901, -19.755 -40.500 43.036, -15.010 -40.500 52.412, -14.177 -40.500 52.698, -19.492 -40.500 42.195, -19.138 -40.500 41.388, -13.317 -40.500 52.891, -18.697 -40.500 40.624, -12.441 -40.500 52.988, -11.559 -40.500 52.988, -9.823 -40.500 52.698, -8.990 -40.500 52.412, -10.683 -40.500 52.891, -18.175 -40.500 39.914, -8.192 -40.500 52.036, -7.442 -40.500 51.574, -6.746 -40.500 51.033, -17.578 -40.500 39.266, -6.114 -40.500 50.418, -16.914 -40.500 38.687, -5.554 -40.500 49.738, -16.189 -40.500 38.185, -5.072 -40.500 49.000, -15.414 -40.500 37.765, -4.674 -40.500 48.214, -14.598 -40.500 37.433, -4.365 -40.500 47.388, -13.749 -40.500 37.194, -4.148 -40.500 46.534, -12.880 -40.500 37.049, -4.027 -40.500 45.661, -12.000 -40.500 37.000, -4.003 -40.500 44.780, -11.120 -40.500 37.049, -4.076 -40.500 43.901, -10.251 -40.500 37.194, -4.245 -40.500 43.036, -9.402 -40.500 37.433, -4.508 -40.500 42.195, -8.586 -40.500 37.765, -4.862 -40.500 41.388, -7.811 -40.500 38.185, -5.303 -40.500 40.624, -7.086 -40.500 38.687, -5.825 -40.500 39.914, -6.422 -40.500 39.266, 14.000 -18.000 8.483, 14.000 -18.000 4.500, 14.000 -17.170 7.786, 13.936 -19.000 4.500, 14.000 15.552 14.795, 14.000 18.000 13.000, 14.000 -14.552 14.795, 14.000 -17.000 13.000, 14.000 19.000 8.483, 14.000 19.000 4.500, 14.000 18.170 7.786, 13.936 20.000 4.500, 13.186 19.444 -4.000, -12.000 44.000 44.996, -12.000 44.000 44.000, -12.249 44.000 44.031, -12.482 44.000 44.124, -12.685 44.000 44.271, -12.844 44.000 44.464, -12.951 44.000 44.691, -12.998 44.000 44.937, -12.982 44.000 45.187, -12.905 44.000 45.426, -12.771 44.000 45.637, -12.588 44.000 45.809, -12.368 44.000 45.930, -12.125 44.000 45.992, -11.875 44.000 45.992, -11.632 44.000 45.930, -11.412 44.000 45.809, -11.229 44.000 45.637, -11.095 44.000 45.426, -11.018 44.000 45.187, -11.002 44.000 44.937, -11.049 44.000 44.691, -11.156 44.000 44.464, -11.315 44.000 44.271, -11.518 44.000 44.124, -11.751 44.000 44.031, -16.632 -3.314 59.000, -19.500 -7.000 59.000, -16.184 -3.801 59.000, -15.685 -4.235 59.000, -17.023 -2.782 59.000, -15.142 -4.612 59.000, -17.354 -2.209 59.000, -14.561 -4.926 59.000, -17.619 -1.604 59.000, -13.948 -5.175 59.000, -17.816 -0.973 59.000, -13.312 -5.355 59.000, -17.943 -0.324 59.000, -12.660 -5.464 59.000, -17.998 0.335 59.000, -12.000 -5.500 59.000, -4.500 -7.000 59.000, -11.340 -5.464 59.000, -10.688 -5.355 59.000, -10.052 -5.175 59.000, -9.439 -4.926 59.000, -8.858 -4.612 59.000, -8.315 -4.235 59.000, -7.816 -3.801 59.000, -19.500 8.000 59.000, -16.835 4.053 59.000, -16.414 4.564 59.000, -17.196 3.500 59.000, -17.495 2.910 59.000, -17.726 2.291 59.000, -17.889 1.650 59.000, -17.980 0.995 59.000, -15.941 5.025 59.000, -7.369 -3.314 59.000, -15.419 5.431 59.000, -6.977 -2.782 59.000, -14.856 5.777 59.000, -6.646 -2.209 59.000, -14.258 6.059 59.000, -6.381 -1.604 59.000, -13.633 6.274 59.000, -6.184 -0.973 59.000, -12.988 6.418 59.000, -6.057 -0.324 59.000, -6.002 0.335 59.000, -4.500 8.000 59.000, -6.020 0.995 59.000, -6.111 1.650 59.000, -6.274 2.291 59.000, -6.505 2.910 59.000, -6.804 3.500 59.000, -7.165 4.053 59.000, -7.586 4.564 59.000, -8.059 5.025 59.000, -8.581 5.431 59.000, -9.144 5.777 59.000, -9.742 6.059 59.000, -10.367 6.274 59.000, -11.012 6.418 59.000, -11.669 6.491 59.000, -12.331 6.491 59.000, -11.120 -40.500 37.049, -12.000 -41.500 37.000, -12.000 -40.500 37.000, -11.104 -41.500 37.050, -10.251 -40.500 37.194, -10.220 -41.500 37.201, -9.402 -40.500 37.433, -9.358 -41.500 37.449, -8.586 -40.500 37.765, -8.529 -41.500 37.792, -7.811 -40.500 38.185, -7.744 -41.500 38.226, -7.086 -40.500 38.687, -7.012 -41.500 38.745, -6.422 -40.500 39.266, -6.343 -41.500 39.343, -5.825 -40.500 39.914, -5.745 -41.500 40.012, -5.303 -40.500 40.624, -5.226 -41.500 40.744, -4.862 -40.500 41.388, -4.792 -41.500 41.529, -4.508 -40.500 42.195, -4.449 -41.500 42.358, -4.245 -40.500 43.036, -4.201 -41.500 43.220, -4.076 -40.500 43.901, -4.050 -41.500 44.104, -4.003 -40.500 44.780, -4.000 -41.500 45.000, -4.027 -40.500 45.661, -4.050 -41.500 45.896, -4.148 -40.500 46.534, -4.201 -41.500 46.780, -4.365 -40.500 47.388, -4.449 -41.500 47.642, -4.674 -40.500 48.214, -4.792 -41.500 48.471, -5.072 -40.500 49.000, -5.226 -41.500 49.256, -5.554 -40.500 49.738, -5.745 -41.500 49.988, -6.114 -40.500 50.418, -6.343 -41.500 50.657, -6.746 -40.500 51.033, -7.012 -41.500 51.255, -7.442 -40.500 51.574, -7.744 -41.500 51.774, -8.192 -40.500 52.036, -8.529 -41.500 52.208, -8.990 -40.500 52.412, -9.358 -41.500 52.551, -9.823 -40.500 52.698, -10.220 -41.500 52.799, -10.683 -40.500 52.891, -11.104 -41.500 52.950, -11.559 -40.500 52.988, -12.000 -41.500 53.000, -12.441 -40.500 52.988, -12.896 -41.500 52.950, -13.317 -40.500 52.891, -13.780 -41.500 52.799, -14.177 -40.500 52.698, -14.642 -41.500 52.551, -15.010 -40.500 52.412, -15.471 -41.500 52.208, -15.808 -40.500 52.036, -16.256 -41.500 51.774, -16.558 -40.500 51.574, -16.988 -41.500 51.255, -17.254 -40.500 51.033, -17.657 -41.500 50.657, -17.886 -40.500 50.418, -18.255 -41.500 49.988, -18.446 -40.500 49.738, -18.774 -41.500 49.256, -18.928 -40.500 49.000, -19.208 -41.500 48.471, -19.326 -40.500 48.214, -19.551 -41.500 47.642, -19.635 -40.500 47.388, -19.799 -41.500 46.780, -19.852 -40.500 46.534, -19.950 -41.500 45.896, -19.973 -40.500 45.661, -20.000 -41.500 45.000, -19.997 -40.500 44.780, -19.950 -41.500 44.104, -19.924 -40.500 43.901, -19.799 -41.500 43.220, -19.755 -40.500 43.036, -19.551 -41.500 42.358, -19.492 -40.500 42.195, -19.208 -41.500 41.529, -19.138 -40.500 41.388, -18.774 -41.500 40.744, -18.697 -40.500 40.624, -18.255 -41.500 40.012, -18.175 -40.500 39.914, -17.657 -41.500 39.343, -17.578 -40.500 39.266, -16.988 -41.500 38.745, -16.914 -40.500 38.687, -16.256 -41.500 38.226, -16.189 -40.500 38.185, -15.471 -41.500 37.792, -15.414 -40.500 37.765, -14.642 -41.500 37.449, -14.598 -40.500 37.433, -13.780 -41.500 37.201, -13.749 -40.500 37.194, -12.896 -41.500 37.050, -12.880 -40.500 37.049, 14.000 -17.378 10.639, 14.000 18.378 10.639, -12.707 -0.207 59.000, -12.866 0.000 59.000, -12.500 -0.366 59.000, -12.966 0.241 59.000, -12.259 -0.466 59.000, -12.000 -0.500 59.000, -13.000 0.500 59.000, -12.966 0.759 59.000, -11.741 -0.466 59.000, -11.500 -0.366 59.000, -12.866 1.000 59.000, -11.293 -0.207 59.000, -12.707 1.207 59.000, -11.134 0.000 59.000, -12.500 1.366 59.000, -12.259 1.466 59.000, -11.034 0.241 59.000, -12.000 1.500 59.000, -11.000 0.500 59.000, -11.034 0.759 59.000, -11.741 1.466 59.000, -11.500 1.366 59.000, -11.134 1.000 59.000, -11.293 1.207 59.000, -11.120 -42.500 37.049, -12.000 -42.500 37.000, -10.251 -42.500 37.194, -9.402 -42.500 37.433, -8.586 -42.500 37.765, -7.811 -42.500 38.185, -7.086 -42.500 38.687, -6.422 -42.500 39.266, -5.825 -42.500 39.914, -5.303 -42.500 40.624, -4.862 -42.500 41.388, -4.508 -42.500 42.195, -4.245 -42.500 43.036, -4.076 -42.500 43.901, -4.003 -42.500 44.780, -4.027 -42.500 45.661, -4.148 -42.500 46.534, -4.365 -42.500 47.388, -4.674 -42.500 48.214, -5.072 -42.500 49.000, -5.554 -42.500 49.738, -6.114 -42.500 50.418, -6.746 -42.500 51.033, -7.442 -42.500 51.574, -8.192 -42.500 52.036, -8.990 -42.500 52.412, -9.823 -42.500 52.698, -10.683 -42.500 52.891, -11.559 -42.500 52.988, -12.441 -42.500 52.988, -13.317 -42.500 52.891, -14.177 -42.500 52.698, -15.010 -42.500 52.412, -15.808 -42.500 52.036, -16.558 -42.500 51.574, -17.254 -42.500 51.033, -17.886 -42.500 50.418, -18.446 -42.500 49.738, -18.928 -42.500 49.000, -19.326 -42.500 48.214, -19.635 -42.500 47.388, -19.852 -42.500 46.534, -19.973 -42.500 45.661, -19.997 -42.500 44.780, -19.924 -42.500 43.901, -19.755 -42.500 43.036, -19.492 -42.500 42.195, -19.138 -42.500 41.388, -18.697 -42.500 40.624, -18.175 -42.500 39.914, -17.578 -42.500 39.266, -16.914 -42.500 38.687, -16.189 -42.500 38.185, -15.414 -42.500 37.765, -14.598 -42.500 37.433, -13.749 -42.500 37.194, -12.880 -42.500 37.049, -12.231 1.473 60.000, -12.000 1.500 60.000, -12.449 1.394 60.000, -12.643 1.266 60.000, -12.802 1.097 60.000, -12.918 0.896 60.000, -12.985 0.674 60.000, -12.998 0.442 60.000, -12.958 0.213 60.000, -12.866 0.000 60.000, -12.727 -0.186 60.000, -12.550 -0.335 60.000, -12.342 -0.440 60.000, -12.116 -0.493 60.000, -11.884 -0.493 60.000, -11.658 -0.440 60.000, -11.450 -0.335 60.000, -11.273 -0.186 60.000, -11.134 0.000 60.000, -11.042 0.213 60.000, -11.002 0.442 60.000, -11.015 0.674 60.000, -11.082 0.896 60.000, -11.198 1.097 60.000, -11.357 1.266 60.000, -11.551 1.394 60.000, -11.769 1.473 60.000, -13.352 -42.500 46.473, -17.254 -42.500 51.033, -17.886 -42.500 50.418, -13.511 -42.500 46.310, -18.446 -42.500 49.738, -13.176 -42.500 46.618, -16.558 -42.500 51.574, -13.651 -42.500 46.129, -18.928 -42.500 49.000, -12.983 -42.500 46.741, -15.808 -42.500 52.036, -13.769 -42.500 45.933, -19.326 -42.500 48.214, -12.779 -42.500 46.842, -15.010 -42.500 52.412, -13.864 -42.500 45.726, -19.635 -42.500 47.388, -12.563 -42.500 46.919, -14.177 -42.500 52.698, -13.934 -42.500 45.508, -19.852 -42.500 46.534, -12.341 -42.500 46.971, -13.317 -42.500 52.891, -13.980 -42.500 45.285, -19.973 -42.500 45.661, -12.114 -42.500 46.997, -11.559 -42.500 52.988, -12.441 -42.500 52.988, -13.999 -42.500 45.057, -19.997 -42.500 44.780, -11.886 -42.500 46.997, -10.683 -42.500 52.891, -13.993 -42.500 44.829, -19.924 -42.500 43.901, -11.659 -42.500 46.971, -9.823 -42.500 52.698, -13.960 -42.500 44.603, -19.755 -42.500 43.036, -11.437 -42.500 46.919, -8.990 -42.500 52.412, -13.902 -42.500 44.382, -19.492 -42.500 42.195, -11.221 -42.500 46.842, -8.192 -42.500 52.036, -13.819 -42.500 44.169, -19.138 -42.500 41.388, -11.017 -42.500 46.741, -7.442 -42.500 51.574, -13.713 -42.500 43.967, -18.697 -42.500 40.624, -10.824 -42.500 46.618, -6.746 -42.500 51.033, -13.584 -42.500 43.779, -18.175 -42.500 39.914, -10.648 -42.500 46.473, -13.434 -42.500 43.606, -17.578 -42.500 39.266, -13.266 -42.500 43.452, -6.114 -42.500 50.418, -10.489 -42.500 46.310, -16.914 -42.500 38.687, -13.081 -42.500 43.317, -5.554 -42.500 49.738, -10.349 -42.500 46.129, -16.189 -42.500 38.185, -12.882 -42.500 43.205, -5.072 -42.500 49.000, -10.231 -42.500 45.933, -15.414 -42.500 37.765, -12.672 -42.500 43.116, -4.674 -42.500 48.214, -10.136 -42.500 45.726, -14.598 -42.500 37.433, -12.453 -42.500 43.052, -4.365 -42.500 47.388, -10.066 -42.500 45.508, -13.749 -42.500 37.194, -12.228 -42.500 43.013, -4.148 -42.500 46.534, -10.020 -42.500 45.285, -12.880 -42.500 37.049, -12.000 -42.500 43.000, -4.027 -42.500 45.661, -10.001 -42.500 45.057, -12.000 -42.500 37.000, -4.003 -42.500 44.780, -10.007 -42.500 44.829, -11.120 -42.500 37.049, -11.772 -42.500 43.013, -4.076 -42.500 43.901, -10.040 -42.500 44.603, -10.251 -42.500 37.194, -11.547 -42.500 43.052, -4.245 -42.500 43.036, -10.098 -42.500 44.382, -9.402 -42.500 37.433, -11.328 -42.500 43.116, -4.508 -42.500 42.195, -10.181 -42.500 44.169, -8.586 -42.500 37.765, -11.118 -42.500 43.205, -4.862 -42.500 41.388, -10.287 -42.500 43.967, -7.811 -42.500 38.185, -10.919 -42.500 43.317, -5.303 -42.500 40.624, -10.416 -42.500 43.779, -7.086 -42.500 38.687, -10.734 -42.500 43.452, -5.825 -42.500 39.914, -10.566 -42.500 43.606, -6.422 -42.500 39.266, -12.000 0.503 60.000, -12.000 1.500 60.000, -12.231 1.473 60.000, -12.449 1.394 60.000, -12.643 1.266 60.000, -12.802 1.097 60.000, -12.918 0.896 60.000, -12.985 0.674 60.000, -12.998 0.442 60.000, -12.958 0.213 60.000, -12.866 0.000 60.000, -12.727 -0.186 60.000, -12.550 -0.335 60.000, -12.342 -0.440 60.000, -12.116 -0.493 60.000, -11.884 -0.493 60.000, -11.658 -0.440 60.000, -11.450 -0.335 60.000, -11.273 -0.186 60.000, -11.134 0.000 60.000, -11.042 0.213 60.000, -11.002 0.442 60.000, -11.015 0.674 60.000, -11.082 0.896 60.000, -11.198 1.097 60.000, -11.357 1.266 60.000, -11.551 1.394 60.000, -11.769 1.473 60.000, -12.707 -42.500 45.707, -12.500 -42.500 45.866, -12.866 -42.500 45.500, -12.259 -42.500 45.966, -12.966 -42.500 45.259, -12.000 -42.500 46.000, -13.000 -42.500 45.000, -11.741 -42.500 45.966, -12.966 -42.500 44.741, -11.500 -42.500 45.866, -12.866 -42.500 44.500, -11.293 -42.500 45.707, -12.707 -42.500 44.293, -12.500 -42.500 44.134, -11.134 -42.500 45.500, -11.034 -42.500 45.259, -12.259 -42.500 44.034, -11.000 -42.500 45.000, -12.000 -42.500 44.000, -11.034 -42.500 44.741, -11.741 -42.500 44.034, -11.500 -42.500 44.134, -11.134 -42.500 44.500, -11.293 -42.500 44.293, -12.249 -44.000 44.031, -12.000 -44.000 44.000, -12.482 -44.000 44.124, -12.685 -44.000 44.271, -12.844 -44.000 44.464, -12.951 -44.000 44.691, -12.998 -44.000 44.937, -12.982 -44.000 45.187, -12.905 -44.000 45.426, -12.771 -44.000 45.637, -12.588 -44.000 45.809, -12.368 -44.000 45.930, -12.125 -44.000 45.992, -11.875 -44.000 45.992, -11.632 -44.000 45.930, -11.412 -44.000 45.809, -11.229 -44.000 45.637, -11.095 -44.000 45.426, -11.018 -44.000 45.187, -11.002 -44.000 44.937, -11.049 -44.000 44.691, -11.156 -44.000 44.464, -11.315 -44.000 44.271, -11.518 -44.000 44.124, -11.751 -44.000 44.031, -12.249 -44.000 44.031, -12.000 -44.000 44.996, -12.482 -44.000 44.124, -12.685 -44.000 44.271, -12.844 -44.000 44.464, -12.951 -44.000 44.691, -12.998 -44.000 44.937, -12.982 -44.000 45.187, -12.905 -44.000 45.426, -12.771 -44.000 45.637, -12.588 -44.000 45.809, -12.368 -44.000 45.930, -12.125 -44.000 45.992, -11.875 -44.000 45.992, -11.632 -44.000 45.930, -11.412 -44.000 45.809, -11.229 -44.000 45.637, -11.095 -44.000 45.426, -11.018 -44.000 45.187, -11.002 -44.000 44.937, -11.049 -44.000 44.691, -11.156 -44.000 44.464, -11.315 -44.000 44.271, -11.518 -44.000 44.124, -11.751 -44.000 44.031 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 0, 2, 3, -1, 4, 5, 6, -1, 7, 4, 6, -1, 8, 4, 7, -1, 9, 4, 8, -1, 10, 9, 8, -1, 11, 9, 10, -1, 3, 2, 12, -1, 3, 12, 13, -1, 14, 15, 16, -1, 14, 16, 17, -1, 18, 19, 20, -1, 19, 17, 20, -1, 20, 21, 22, -1, 18, 20, 22, -1, 21, 23, 24, -1, 22, 21, 24, -1, 23, 25, 26, -1, 24, 23, 26, -1, 17, 19, 14, -1, 27, 28, 1, -1, 27, 1, 0, -1, 29, 30, 28, -1, 31, 29, 28, -1, 32, 29, 33, -1, 32, 33, 34, -1, 32, 34, 30, -1, 32, 30, 29, -1, 35, 36, 12, -1, 12, 36, 13, -1, 37, 9, 38, -1, 37, 38, 39, -1, 40, 4, 9, -1, 40, 9, 41, -1, 42, 43, 44, -1, 45, 4, 40, -1, 42, 46, 43, -1, 42, 47, 46, -1, 42, 4, 47, -1, 48, 49, 50, -1, 48, 51, 49, -1, 48, 52, 51, -1, 48, 53, 52, -1, 54, 4, 45, -1, 48, 50, 37, -1, 55, 56, 53, -1, 55, 57, 56, -1, 55, 44, 57, -1, 55, 42, 44, -1, 58, 4, 54, -1, 55, 53, 48, -1, 59, 60, 41, -1, 59, 61, 60, -1, 59, 62, 61, -1, 59, 63, 62, -1, 59, 64, 63, -1, 59, 65, 64, -1, 59, 66, 65, -1, 59, 67, 66, -1, 59, 50, 67, -1, 59, 9, 37, -1, 59, 41, 9, -1, 47, 4, 58, -1, 59, 37, 50, -1, 68, 69, 70, -1, 71, 72, 68, -1, 71, 73, 72, -1, 71, 70, 73, -1, 71, 68, 70, -1, 74, 75, 70, -1, 76, 77, 74, -1, 76, 78, 77, -1, 76, 70, 78, -1, 76, 74, 70, -1, 79, 80, 81, -1, 79, 81, 82, -1, 79, 83, 80, -1, 79, 82, 84, -1, 79, 84, 83, -1, 81, 80, 85, -1, 80, 86, 85, -1, 85, 86, 87, -1, 85, 87, 88, -1, 87, 89, 88, -1, 89, 90, 88, -1, 89, 91, 90, -1, 91, 92, 90, -1, 93, 94, 95, -1, 96, 97, 98, -1, 96, 94, 97, -1, 96, 98, 95, -1, 96, 95, 94, -1, 94, 93, 72, -1, 73, 94, 72, -1, 99, 100, 101, -1, 102, 100, 99, -1, 84, 102, 99, -1, 83, 84, 99, -1, 98, 97, 33, -1, 97, 34, 33, -1, 97, 103, 34, -1, 30, 34, 104, -1, 34, 103, 104, -1, 50, 105, 67, -1, 106, 107, 54, -1, 108, 105, 50, -1, 49, 108, 50, -1, 40, 109, 45, -1, 109, 54, 45, -1, 109, 106, 54, -1, 110, 108, 49, -1, 111, 109, 40, -1, 51, 110, 49, -1, 112, 110, 51, -1, 41, 111, 40, -1, 52, 112, 51, -1, 113, 111, 41, -1, 114, 112, 52, -1, 60, 113, 41, -1, 53, 114, 52, -1, 115, 113, 60, -1, 116, 114, 53, -1, 61, 115, 60, -1, 56, 116, 53, -1, 117, 115, 61, -1, 118, 116, 56, -1, 57, 118, 56, -1, 62, 117, 61, -1, 119, 117, 62, -1, 120, 118, 57, -1, 44, 120, 57, -1, 63, 119, 62, -1, 121, 120, 44, -1, 122, 119, 63, -1, 64, 122, 63, -1, 123, 44, 43, -1, 123, 121, 44, -1, 124, 122, 64, -1, 65, 124, 64, -1, 125, 43, 46, -1, 125, 123, 43, -1, 126, 124, 65, -1, 66, 126, 65, -1, 127, 46, 47, -1, 127, 125, 46, -1, 128, 126, 66, -1, 58, 127, 47, -1, 67, 128, 66, -1, 107, 127, 58, -1, 105, 128, 67, -1, 54, 107, 58, -1, 129, 130, 74, -1, 77, 129, 74, -1, 131, 132, 133, -1, 134, 132, 131, -1, 135, 134, 131, -1, 136, 131, 129, -1, 136, 77, 135, -1, 136, 129, 77, -1, 136, 135, 131, -1, 137, 138, 139, -1, 138, 140, 141, -1, 142, 139, 141, -1, 140, 142, 141, -1, 139, 138, 141, -1, 143, 144, 145, -1, 145, 48, 146, -1, 143, 145, 146, -1, 146, 48, 147, -1, 147, 37, 148, -1, 148, 37, 149, -1, 147, 48, 37, -1, 150, 151, 152, -1, 153, 150, 154, -1, 153, 151, 150, -1, 155, 104, 156, -1, 155, 153, 104, -1, 70, 75, 73, -1, 75, 94, 73, -1, 157, 158, 159, -1, 157, 159, 160, -1, 158, 161, 134, -1, 161, 132, 134, -1, 97, 94, 162, -1, 163, 130, 164, -1, 165, 163, 164, -1, 158, 134, 159, -1, 157, 166, 158, -1, 75, 167, 162, -1, 94, 75, 162, -1, 164, 168, 169, -1, 103, 97, 169, -1, 97, 162, 165, -1, 170, 103, 169, -1, 171, 170, 169, -1, 97, 165, 169, -1, 165, 164, 169, -1, 172, 171, 169, -1, 168, 172, 169, -1, 163, 167, 74, -1, 167, 75, 74, -1, 163, 74, 130, -1, 171, 172, 166, -1, 171, 166, 157, -1, 168, 132, 161, -1, 172, 168, 161, -1, 173, 174, 175, -1, 176, 177, 178, -1, 178, 179, 180, -1, 173, 175, 181, -1, 176, 182, 183, -1, 176, 183, 184, -1, 173, 181, 185, -1, 183, 182, 186, -1, 178, 180, 187, -1, 176, 184, 188, -1, 186, 182, 189, -1, 176, 188, 190, -1, 178, 187, 191, -1, 189, 182, 192, -1, 193, 194, 195, -1, 194, 196, 195, -1, 196, 197, 195, -1, 192, 182, 198, -1, 199, 193, 200, -1, 193, 195, 200, -1, 176, 190, 177, -1, 185, 201, 202, -1, 201, 203, 202, -1, 203, 204, 202, -1, 204, 199, 202, -1, 173, 185, 202, -1, 199, 200, 202, -1, 205, 206, 207, -1, 206, 198, 207, -1, 196, 208, 197, -1, 208, 209, 197, -1, 209, 210, 197, -1, 210, 211, 197, -1, 207, 198, 212, -1, 211, 182, 197, -1, 198, 182, 212, -1, 191, 213, 173, -1, 213, 174, 173, -1, 178, 191, 173, -1, 212, 182, 214, -1, 214, 182, 211, -1, 177, 179, 178, -1, 82, 102, 84, -1, 82, 215, 102, -1, 216, 101, 100, -1, 217, 216, 92, -1, 217, 92, 91, -1, 217, 91, 101, -1, 217, 101, 216, -1, 102, 216, 100, -1, 215, 216, 102, -1, 103, 218, 219, -1, 156, 104, 220, -1, 219, 220, 221, -1, 104, 103, 221, -1, 103, 219, 221, -1, 220, 104, 221, -1, 222, 223, 128, -1, 224, 222, 128, -1, 225, 226, 126, -1, 227, 225, 126, -1, 223, 227, 126, -1, 128, 223, 126, -1, 228, 224, 105, -1, 229, 228, 105, -1, 224, 128, 105, -1, 126, 226, 124, -1, 230, 231, 124, -1, 226, 230, 124, -1, 232, 229, 108, -1, 233, 232, 108, -1, 234, 233, 108, -1, 229, 105, 108, -1, 235, 236, 110, -1, 234, 108, 110, -1, 236, 234, 110, -1, 124, 231, 122, -1, 237, 238, 122, -1, 231, 237, 122, -1, 122, 238, 119, -1, 239, 240, 119, -1, 241, 239, 119, -1, 238, 241, 119, -1, 242, 235, 112, -1, 243, 242, 112, -1, 244, 243, 112, -1, 235, 110, 112, -1, 245, 244, 114, -1, 244, 112, 114, -1, 246, 245, 114, -1, 119, 240, 117, -1, 247, 248, 117, -1, 240, 247, 117, -1, 249, 246, 116, -1, 246, 114, 116, -1, 117, 248, 115, -1, 248, 250, 115, -1, 115, 250, 251, -1, 116, 118, 252, -1, 249, 116, 252, -1, 113, 115, 253, -1, 115, 251, 253, -1, 252, 118, 254, -1, 113, 253, 255, -1, 254, 118, 256, -1, 111, 113, 257, -1, 113, 255, 257, -1, 118, 120, 258, -1, 256, 118, 258, -1, 111, 257, 259, -1, 258, 120, 260, -1, 111, 259, 261, -1, 109, 111, 261, -1, 260, 120, 262, -1, 120, 121, 262, -1, 109, 261, 263, -1, 262, 121, 264, -1, 109, 263, 265, -1, 121, 123, 266, -1, 264, 121, 266, -1, 109, 265, 267, -1, 106, 109, 267, -1, 266, 123, 268, -1, 106, 267, 269, -1, 268, 123, 270, -1, 106, 269, 271, -1, 107, 106, 271, -1, 123, 125, 272, -1, 270, 123, 272, -1, 107, 271, 273, -1, 272, 125, 274, -1, 127, 107, 275, -1, 107, 273, 275, -1, 125, 127, 276, -1, 274, 125, 276, -1, 127, 275, 277, -1, 276, 127, 278, -1, 127, 277, 278, -1, 279, 280, 130, -1, 129, 279, 130, -1, 281, 279, 129, -1, 133, 282, 281, -1, 131, 133, 281, -1, 131, 281, 129, -1, 164, 283, 284, -1, 283, 164, 130, -1, 164, 284, 168, -1, 283, 130, 280, -1, 284, 282, 133, -1, 284, 133, 132, -1, 168, 284, 132, -1, 285, 140, 286, -1, 140, 138, 286, -1, 287, 288, 289, -1, 290, 287, 289, -1, 288, 291, 292, -1, 289, 288, 292, -1, 293, 287, 290, -1, 294, 293, 290, -1, 291, 154, 295, -1, 291, 295, 292, -1, 296, 295, 297, -1, 150, 296, 297, -1, 154, 150, 297, -1, 295, 154, 297, -1, 293, 298, 299, -1, 294, 298, 293, -1, 286, 300, 298, -1, 298, 301, 302, -1, 301, 303, 302, -1, 303, 286, 302, -1, 286, 298, 302, -1, 296, 152, 304, -1, 296, 150, 152, -1, 305, 306, 151, -1, 307, 153, 155, -1, 308, 307, 305, -1, 308, 151, 153, -1, 308, 305, 151, -1, 308, 153, 307, -1, 155, 156, 220, -1, 155, 220, 307, -1, 309, 285, 286, -1, 309, 310, 285, -1, 309, 303, 310, -1, 309, 286, 303, -1, 151, 218, 103, -1, 151, 306, 218, -1, 311, 310, 312, -1, 304, 152, 313, -1, 152, 310, 313, -1, 311, 304, 313, -1, 310, 311, 313, -1, 314, 315, 316, -1, 317, 318, 319, -1, 320, 314, 321, -1, 322, 323, 324, -1, 322, 324, 325, -1, 317, 324, 318, -1, 326, 314, 327, -1, 326, 328, 315, -1, 326, 329, 328, -1, 326, 330, 329, -1, 326, 331, 330, -1, 326, 332, 331, -1, 326, 333, 332, -1, 326, 315, 314, -1, 326, 324, 333, -1, 326, 327, 324, -1, 324, 334, 333, -1, 324, 323, 334, -1, 318, 324, 327, -1, 314, 335, 321, -1, 314, 316, 335, -1, 172, 336, 337, -1, 166, 172, 337, -1, 319, 338, 317, -1, 319, 339, 338, -1, 340, 341, 342, -1, 343, 344, 345, -1, 344, 346, 345, -1, 346, 347, 345, -1, 339, 342, 338, -1, 343, 345, 348, -1, 341, 349, 350, -1, 351, 341, 350, -1, 342, 341, 338, -1, 340, 345, 352, -1, 347, 353, 352, -1, 353, 354, 352, -1, 354, 355, 352, -1, 355, 356, 352, -1, 356, 357, 352, -1, 357, 358, 352, -1, 345, 347, 352, -1, 358, 341, 352, -1, 341, 340, 352, -1, 358, 359, 341, -1, 359, 349, 341, -1, 360, 165, 361, -1, 362, 363, 364, -1, 362, 365, 363, -1, 362, 366, 365, -1, 163, 360, 367, -1, 368, 362, 364, -1, 369, 370, 165, -1, 369, 165, 371, -1, 163, 165, 360, -1, 372, 362, 361, -1, 372, 373, 366, -1, 372, 374, 373, -1, 372, 375, 374, -1, 372, 376, 375, -1, 372, 377, 376, -1, 372, 378, 377, -1, 372, 366, 362, -1, 372, 165, 378, -1, 372, 361, 165, -1, 165, 379, 378, -1, 165, 370, 379, -1, 162, 380, 371, -1, 165, 162, 371, -1, 367, 167, 163, -1, 367, 381, 167, -1, 382, 162, 383, -1, 384, 385, 386, -1, 385, 387, 386, -1, 387, 388, 386, -1, 381, 383, 167, -1, 384, 386, 389, -1, 162, 390, 391, -1, 380, 162, 391, -1, 383, 162, 167, -1, 382, 386, 392, -1, 388, 393, 392, -1, 393, 394, 392, -1, 394, 395, 392, -1, 395, 396, 392, -1, 396, 397, 392, -1, 397, 398, 392, -1, 386, 388, 392, -1, 398, 162, 392, -1, 162, 382, 392, -1, 398, 399, 162, -1, 399, 390, 162, -1, 208, 400, 401, -1, 208, 401, 209, -1, 209, 401, 402, -1, 209, 402, 210, -1, 210, 402, 403, -1, 210, 403, 211, -1, 211, 403, 404, -1, 211, 404, 405, -1, 406, 201, 407, -1, 405, 404, 408, -1, 408, 404, 409, -1, 201, 185, 410, -1, 407, 201, 410, -1, 411, 406, 412, -1, 409, 404, 413, -1, 406, 407, 412, -1, 185, 414, 415, -1, 410, 185, 415, -1, 199, 411, 416, -1, 193, 199, 416, -1, 411, 412, 416, -1, 415, 414, 417, -1, 194, 193, 418, -1, 193, 416, 418, -1, 196, 194, 400, -1, 194, 418, 400, -1, 196, 400, 208, -1, 419, 420, 421, -1, 422, 419, 423, -1, 422, 420, 419, -1, 424, 422, 423, -1, 425, 424, 426, -1, 425, 422, 424, -1, 427, 425, 426, -1, 428, 425, 427, -1, 428, 427, 429, -1, 430, 428, 429, -1, 431, 430, 432, -1, 431, 428, 430, -1, 433, 431, 432, -1, 434, 431, 433, -1, 435, 431, 434, -1, 436, 435, 434, -1, 437, 435, 436, -1, 438, 435, 437, -1, 439, 438, 437, -1, 440, 438, 439, -1, 441, 438, 440, -1, 442, 441, 440, -1, 443, 441, 442, -1, 444, 443, 445, -1, 444, 441, 443, -1, 446, 444, 445, -1, 447, 446, 448, -1, 447, 444, 446, -1, 449, 447, 448, -1, 450, 449, 451, -1, 450, 447, 449, -1, 452, 450, 451, -1, 453, 452, 454, -1, 453, 450, 452, -1, 455, 453, 454, -1, 456, 455, 457, -1, 456, 453, 455, -1, 458, 456, 457, -1, 459, 456, 458, -1, 460, 456, 459, -1, 461, 460, 459, -1, 462, 460, 461, -1, 417, 460, 462, -1, 463, 417, 462, -1, 464, 417, 463, -1, 415, 417, 464, -1, 465, 415, 464, -1, 466, 415, 465, -1, 410, 415, 466, -1, 467, 410, 466, -1, 468, 410, 467, -1, 407, 410, 468, -1, 469, 407, 468, -1, 470, 407, 469, -1, 412, 407, 470, -1, 471, 412, 470, -1, 472, 412, 471, -1, 416, 412, 472, -1, 416, 472, 473, -1, 474, 416, 473, -1, 418, 416, 474, -1, 418, 474, 475, -1, 476, 418, 475, -1, 400, 418, 476, -1, 400, 476, 477, -1, 478, 400, 477, -1, 401, 400, 478, -1, 401, 478, 479, -1, 480, 401, 479, -1, 402, 401, 480, -1, 402, 480, 481, -1, 482, 402, 481, -1, 403, 402, 482, -1, 403, 482, 483, -1, 484, 403, 483, -1, 404, 403, 484, -1, 404, 484, 485, -1, 486, 404, 485, -1, 413, 404, 486, -1, 413, 486, 487, -1, 488, 413, 487, -1, 488, 489, 413, -1, 490, 489, 488, -1, 421, 490, 488, -1, 420, 490, 421, -1, 220, 219, 305, -1, 220, 305, 307, -1, 219, 218, 306, -1, 305, 219, 306, -1, 222, 491, 223, -1, 491, 222, 224, -1, 223, 491, 227, -1, 491, 224, 228, -1, 227, 491, 225, -1, 491, 228, 229, -1, 225, 491, 226, -1, 491, 229, 232, -1, 226, 491, 230, -1, 491, 232, 233, -1, 230, 491, 231, -1, 491, 233, 234, -1, 231, 491, 237, -1, 491, 234, 236, -1, 236, 235, 492, -1, 235, 242, 492, -1, 242, 243, 492, -1, 243, 244, 492, -1, 244, 245, 492, -1, 245, 246, 492, -1, 246, 249, 492, -1, 491, 236, 492, -1, 250, 248, 493, -1, 248, 247, 493, -1, 247, 240, 493, -1, 240, 239, 493, -1, 239, 241, 493, -1, 241, 238, 493, -1, 238, 237, 493, -1, 237, 491, 493, -1, 250, 493, 251, -1, 492, 249, 252, -1, 251, 493, 253, -1, 492, 252, 254, -1, 253, 493, 255, -1, 492, 254, 256, -1, 255, 493, 257, -1, 492, 256, 258, -1, 257, 493, 259, -1, 492, 258, 260, -1, 259, 493, 261, -1, 492, 260, 262, -1, 492, 262, 264, -1, 264, 266, 494, -1, 266, 268, 494, -1, 268, 270, 494, -1, 270, 272, 494, -1, 272, 274, 494, -1, 274, 276, 494, -1, 276, 278, 494, -1, 278, 277, 494, -1, 277, 275, 494, -1, 275, 273, 494, -1, 273, 271, 494, -1, 271, 269, 494, -1, 269, 267, 494, -1, 492, 264, 494, -1, 493, 267, 265, -1, 263, 493, 265, -1, 261, 493, 263, -1, 494, 267, 493, -1, 281, 280, 279, -1, 281, 282, 280, -1, 284, 283, 282, -1, 283, 280, 282, -1, 495, 496, 497, -1, 498, 495, 497, -1, 499, 500, 501, -1, 499, 498, 500, -1, 502, 503, 499, -1, 502, 499, 501, -1, 504, 505, 503, -1, 504, 503, 502, -1, 506, 507, 505, -1, 506, 505, 504, -1, 497, 500, 498, -1, 304, 508, 509, -1, 509, 508, 510, -1, 509, 510, 511, -1, 509, 511, 512, -1, 312, 310, 303, -1, 301, 312, 303, -1, 508, 304, 513, -1, 514, 513, 515, -1, 516, 514, 515, -1, 304, 516, 515, -1, 513, 304, 515, -1, 517, 518, 327, -1, 518, 318, 327, -1, 519, 520, 362, -1, 327, 521, 517, -1, 314, 521, 327, -1, 345, 340, 522, -1, 523, 345, 522, -1, 524, 314, 345, -1, 524, 521, 314, -1, 525, 524, 345, -1, 525, 345, 523, -1, 526, 525, 523, -1, 362, 527, 361, -1, 362, 526, 527, -1, 528, 382, 529, -1, 528, 386, 382, -1, 520, 526, 362, -1, 520, 525, 526, -1, 519, 362, 386, -1, 519, 386, 528, -1, 348, 345, 314, -1, 320, 348, 314, -1, 530, 531, 532, -1, 530, 533, 531, -1, 534, 530, 532, -1, 535, 534, 532, -1, 536, 534, 535, -1, 537, 536, 535, -1, 538, 536, 537, -1, 539, 540, 538, -1, 539, 538, 537, -1, 541, 542, 543, -1, 544, 545, 540, -1, 544, 540, 539, -1, 337, 545, 544, -1, 546, 547, 542, -1, 546, 542, 541, -1, 548, 543, 549, -1, 548, 541, 543, -1, 550, 551, 547, -1, 550, 547, 546, -1, 552, 549, 553, -1, 552, 548, 549, -1, 554, 555, 551, -1, 554, 551, 550, -1, 533, 553, 556, -1, 533, 556, 531, -1, 533, 552, 553, -1, 518, 557, 318, -1, 318, 557, 319, -1, 558, 342, 339, -1, 558, 339, 559, -1, 339, 319, 560, -1, 319, 557, 560, -1, 339, 560, 561, -1, 559, 339, 561, -1, 559, 561, 562, -1, 360, 563, 367, -1, 563, 562, 367, -1, 564, 383, 565, -1, 383, 381, 565, -1, 367, 562, 566, -1, 562, 561, 566, -1, 381, 367, 567, -1, 565, 381, 567, -1, 367, 566, 567, -1, 568, 569, 570, -1, 568, 571, 569, -1, 572, 337, 336, -1, 573, 568, 570, -1, 574, 568, 573, -1, 575, 337, 572, -1, 576, 574, 573, -1, 577, 575, 572, -1, 578, 574, 576, -1, 579, 575, 577, -1, 555, 576, 580, -1, 581, 579, 577, -1, 555, 578, 576, -1, 582, 579, 581, -1, 583, 581, 584, -1, 583, 582, 581, -1, 585, 583, 584, -1, 586, 583, 585, -1, 587, 585, 588, -1, 587, 586, 585, -1, 589, 588, 590, -1, 589, 587, 588, -1, 591, 589, 590, -1, 592, 589, 591, -1, 593, 591, 594, -1, 593, 592, 591, -1, 569, 593, 594, -1, 571, 593, 569, -1, 340, 342, 522, -1, 342, 558, 522, -1, 595, 596, 597, -1, 596, 598, 597, -1, 595, 597, 599, -1, 595, 599, 600, -1, 600, 599, 601, -1, 600, 601, 602, -1, 602, 601, 603, -1, 603, 604, 605, -1, 602, 603, 605, -1, 606, 607, 608, -1, 604, 609, 610, -1, 605, 604, 610, -1, 607, 611, 612, -1, 610, 609, 336, -1, 608, 607, 612, -1, 613, 606, 614, -1, 606, 608, 614, -1, 611, 615, 616, -1, 612, 611, 616, -1, 617, 613, 618, -1, 613, 614, 618, -1, 615, 580, 619, -1, 616, 615, 619, -1, 620, 617, 598, -1, 596, 620, 598, -1, 617, 618, 598, -1, 563, 360, 527, -1, 527, 360, 361, -1, 389, 386, 362, -1, 368, 389, 362, -1, 621, 376, 377, -1, 621, 622, 376, -1, 623, 621, 377, -1, 624, 623, 377, -1, 625, 623, 624, -1, 626, 625, 624, -1, 627, 625, 626, -1, 628, 629, 627, -1, 628, 627, 626, -1, 630, 631, 632, -1, 633, 634, 629, -1, 633, 629, 628, -1, 371, 634, 633, -1, 635, 636, 631, -1, 635, 631, 630, -1, 637, 632, 638, -1, 637, 630, 632, -1, 639, 640, 636, -1, 639, 636, 635, -1, 641, 638, 374, -1, 641, 637, 638, -1, 642, 368, 640, -1, 642, 640, 639, -1, 622, 374, 643, -1, 622, 643, 376, -1, 622, 641, 374, -1, 644, 645, 646, -1, 644, 647, 645, -1, 648, 371, 380, -1, 649, 644, 646, -1, 650, 644, 649, -1, 651, 371, 648, -1, 652, 650, 649, -1, 653, 651, 648, -1, 654, 650, 652, -1, 655, 651, 653, -1, 368, 652, 389, -1, 656, 655, 653, -1, 368, 654, 652, -1, 657, 655, 656, -1, 658, 656, 659, -1, 658, 657, 656, -1, 660, 658, 659, -1, 661, 658, 660, -1, 662, 660, 663, -1, 662, 661, 660, -1, 664, 663, 665, -1, 664, 662, 663, -1, 666, 664, 665, -1, 667, 664, 666, -1, 668, 666, 669, -1, 668, 667, 666, -1, 645, 668, 669, -1, 647, 668, 645, -1, 382, 383, 529, -1, 383, 564, 529, -1, 397, 396, 670, -1, 396, 671, 670, -1, 397, 670, 672, -1, 397, 672, 673, -1, 673, 672, 674, -1, 673, 674, 675, -1, 675, 674, 676, -1, 676, 677, 678, -1, 675, 676, 678, -1, 679, 680, 681, -1, 677, 682, 683, -1, 678, 677, 683, -1, 680, 684, 685, -1, 683, 682, 380, -1, 681, 680, 685, -1, 686, 679, 687, -1, 679, 681, 687, -1, 684, 688, 689, -1, 685, 684, 689, -1, 394, 686, 690, -1, 686, 687, 690, -1, 688, 389, 691, -1, 689, 688, 691, -1, 692, 394, 671, -1, 396, 692, 671, -1, 394, 690, 671, -1, 693, 421, 694, -1, 419, 421, 693, -1, 695, 419, 693, -1, 423, 419, 695, -1, 696, 423, 695, -1, 424, 423, 696, -1, 697, 424, 696, -1, 426, 424, 697, -1, 698, 426, 697, -1, 427, 426, 698, -1, 699, 427, 698, -1, 429, 427, 699, -1, 700, 429, 699, -1, 430, 429, 700, -1, 701, 430, 700, -1, 432, 430, 701, -1, 702, 432, 701, -1, 433, 432, 702, -1, 703, 433, 702, -1, 434, 433, 703, -1, 704, 434, 703, -1, 436, 434, 704, -1, 705, 436, 704, -1, 437, 436, 705, -1, 706, 437, 705, -1, 439, 437, 706, -1, 707, 439, 706, -1, 440, 439, 707, -1, 708, 440, 707, -1, 442, 440, 708, -1, 709, 442, 708, -1, 443, 442, 709, -1, 710, 443, 709, -1, 445, 443, 710, -1, 711, 445, 710, -1, 446, 445, 711, -1, 712, 446, 711, -1, 448, 446, 712, -1, 713, 448, 712, -1, 449, 448, 713, -1, 714, 449, 713, -1, 451, 449, 714, -1, 715, 451, 714, -1, 452, 451, 715, -1, 716, 452, 715, -1, 454, 452, 716, -1, 717, 454, 716, -1, 455, 454, 717, -1, 718, 455, 717, -1, 457, 455, 718, -1, 719, 457, 718, -1, 458, 457, 719, -1, 720, 458, 719, -1, 459, 458, 720, -1, 721, 459, 720, -1, 461, 459, 721, -1, 722, 461, 721, -1, 462, 461, 722, -1, 723, 462, 722, -1, 463, 462, 723, -1, 724, 463, 723, -1, 464, 463, 724, -1, 725, 464, 724, -1, 465, 464, 725, -1, 726, 465, 725, -1, 466, 465, 726, -1, 727, 466, 726, -1, 467, 466, 727, -1, 728, 467, 727, -1, 468, 467, 728, -1, 729, 468, 728, -1, 469, 468, 729, -1, 730, 469, 729, -1, 470, 469, 730, -1, 731, 470, 730, -1, 471, 470, 731, -1, 732, 471, 731, -1, 472, 471, 732, -1, 733, 472, 732, -1, 473, 472, 733, -1, 734, 473, 733, -1, 474, 473, 734, -1, 735, 474, 734, -1, 475, 474, 735, -1, 736, 475, 735, -1, 476, 475, 736, -1, 737, 476, 736, -1, 477, 476, 737, -1, 738, 477, 737, -1, 478, 477, 738, -1, 739, 478, 738, -1, 479, 478, 739, -1, 740, 479, 739, -1, 480, 479, 740, -1, 741, 480, 740, -1, 481, 480, 741, -1, 742, 481, 741, -1, 482, 481, 742, -1, 743, 482, 742, -1, 483, 482, 743, -1, 744, 483, 743, -1, 484, 483, 744, -1, 484, 744, 745, -1, 485, 484, 745, -1, 485, 745, 746, -1, 486, 485, 746, -1, 486, 746, 747, -1, 487, 486, 747, -1, 487, 747, 748, -1, 488, 487, 748, -1, 488, 748, 749, -1, 421, 488, 749, -1, 421, 749, 694, -1, 750, 751, 752, -1, 753, 750, 752, -1, 754, 752, 755, -1, 756, 754, 755, -1, 751, 757, 758, -1, 758, 757, 759, -1, 758, 760, 761, -1, 755, 758, 761, -1, 762, 763, 764, -1, 765, 762, 764, -1, 763, 512, 764, -1, 511, 765, 764, -1, 512, 511, 764, -1, 766, 763, 762, -1, 766, 767, 763, -1, 768, 767, 766, -1, 768, 769, 767, -1, 768, 770, 769, -1, 768, 771, 770, -1, 771, 516, 770, -1, 771, 514, 516, -1, 508, 513, 510, -1, 510, 513, 772, -1, 510, 772, 511, -1, 511, 772, 765, -1, 773, 774, 775, -1, 773, 776, 774, -1, 777, 774, 776, -1, 778, 773, 775, -1, 779, 774, 777, -1, 780, 778, 775, -1, 781, 774, 779, -1, 782, 780, 775, -1, 783, 774, 781, -1, 784, 782, 775, -1, 785, 774, 783, -1, 786, 784, 775, -1, 787, 774, 785, -1, 788, 786, 775, -1, 789, 774, 787, -1, 790, 788, 775, -1, 791, 774, 789, -1, 792, 774, 791, -1, 793, 794, 790, -1, 793, 790, 775, -1, 795, 794, 793, -1, 796, 795, 793, -1, 797, 796, 793, -1, 798, 797, 793, -1, 799, 798, 793, -1, 800, 801, 802, -1, 800, 803, 801, -1, 800, 804, 803, -1, 800, 805, 804, -1, 800, 806, 805, -1, 800, 807, 806, -1, 800, 808, 807, -1, 800, 809, 808, -1, 800, 792, 809, -1, 800, 774, 792, -1, 810, 811, 799, -1, 810, 812, 811, -1, 810, 813, 812, -1, 810, 814, 813, -1, 810, 815, 814, -1, 810, 799, 793, -1, 816, 800, 802, -1, 817, 815, 810, -1, 818, 817, 810, -1, 819, 818, 810, -1, 820, 819, 810, -1, 821, 822, 823, -1, 821, 824, 822, -1, 821, 816, 824, -1, 821, 800, 816, -1, 825, 826, 827, -1, 825, 823, 826, -1, 825, 821, 823, -1, 828, 829, 820, -1, 828, 830, 829, -1, 828, 831, 830, -1, 828, 832, 831, -1, 828, 833, 832, -1, 828, 834, 833, -1, 828, 835, 834, -1, 828, 836, 835, -1, 828, 837, 836, -1, 828, 827, 837, -1, 828, 820, 810, -1, 828, 825, 827, -1, 838, 518, 839, -1, 840, 838, 839, -1, 841, 839, 842, -1, 843, 839, 841, -1, 844, 845, 846, -1, 844, 846, 847, -1, 844, 848, 845, -1, 844, 847, 841, -1, 844, 842, 848, -1, 844, 841, 842, -1, 845, 849, 850, -1, 845, 848, 851, -1, 848, 852, 851, -1, 852, 853, 851, -1, 853, 849, 851, -1, 849, 845, 851, -1, 852, 854, 853, -1, 855, 856, 857, -1, 855, 857, 858, -1, 859, 860, 861, -1, 852, 860, 862, -1, 859, 857, 862, -1, 856, 854, 862, -1, 854, 852, 862, -1, 857, 856, 862, -1, 860, 859, 862, -1, 863, 864, 865, -1, 866, 865, 867, -1, 864, 861, 867, -1, 860, 866, 867, -1, 861, 860, 867, -1, 865, 864, 867, -1, 868, 869, 870, -1, 866, 868, 870, -1, 869, 871, 870, -1, 865, 866, 870, -1, 871, 872, 870, -1, 872, 865, 870, -1, 868, 873, 869, -1, 869, 873, 874, -1, 875, 876, 877, -1, 876, 878, 877, -1, 879, 880, 876, -1, 879, 876, 875, -1, 881, 880, 879, -1, 881, 882, 880, -1, 589, 592, 883, -1, 575, 545, 337, -1, 575, 884, 545, -1, 554, 578, 555, -1, 587, 883, 885, -1, 587, 589, 883, -1, 886, 574, 578, -1, 886, 578, 554, -1, 579, 887, 884, -1, 579, 884, 575, -1, 888, 574, 886, -1, 586, 885, 889, -1, 586, 587, 885, -1, 568, 574, 888, -1, 583, 889, 890, -1, 891, 568, 888, -1, 583, 586, 889, -1, 571, 568, 891, -1, 582, 890, 887, -1, 582, 583, 890, -1, 582, 887, 579, -1, 892, 571, 891, -1, 593, 892, 893, -1, 593, 571, 892, -1, 592, 893, 883, -1, 592, 593, 893, -1, 608, 541, 548, -1, 894, 890, 889, -1, 895, 894, 889, -1, 618, 614, 552, -1, 614, 548, 552, -1, 895, 889, 885, -1, 896, 897, 893, -1, 897, 883, 893, -1, 898, 895, 885, -1, 898, 885, 897, -1, 598, 618, 533, -1, 618, 552, 533, -1, 896, 893, 899, -1, 897, 885, 883, -1, 899, 893, 892, -1, 597, 598, 530, -1, 598, 533, 530, -1, 899, 892, 900, -1, 900, 892, 891, -1, 599, 597, 534, -1, 597, 530, 534, -1, 900, 891, 901, -1, 599, 534, 601, -1, 901, 891, 888, -1, 601, 534, 536, -1, 901, 888, 902, -1, 603, 601, 538, -1, 902, 888, 886, -1, 601, 536, 538, -1, 902, 886, 619, -1, 604, 603, 540, -1, 603, 538, 540, -1, 619, 886, 554, -1, 616, 619, 550, -1, 609, 604, 545, -1, 604, 540, 545, -1, 619, 554, 550, -1, 609, 545, 903, -1, 612, 616, 546, -1, 903, 545, 884, -1, 616, 550, 546, -1, 903, 884, 904, -1, 608, 612, 541, -1, 904, 884, 887, -1, 612, 546, 541, -1, 904, 887, 890, -1, 894, 904, 890, -1, 614, 608, 548, -1, 838, 905, 906, -1, 518, 838, 906, -1, 905, 907, 906, -1, 908, 518, 906, -1, 907, 908, 906, -1, 909, 910, 911, -1, 912, 909, 911, -1, 913, 909, 914, -1, 915, 913, 914, -1, 557, 915, 914, -1, 560, 557, 914, -1, 912, 560, 914, -1, 909, 912, 914, -1, 916, 917, 918, -1, 919, 920, 921, -1, 921, 920, 922, -1, 920, 916, 923, -1, 918, 922, 923, -1, 916, 918, 923, -1, 922, 920, 923, -1, 921, 922, 924, -1, 925, 921, 924, -1, 926, 925, 927, -1, 927, 925, 924, -1, 928, 929, 930, -1, 924, 929, 931, -1, 928, 927, 931, -1, 927, 924, 931, -1, 929, 928, 931, -1, 932, 933, 934, -1, 933, 935, 934, -1, 567, 932, 936, -1, 565, 567, 936, -1, 937, 565, 936, -1, 938, 937, 936, -1, 934, 938, 936, -1, 932, 934, 936, -1, 939, 940, 941, -1, 942, 939, 941, -1, 940, 943, 941, -1, 944, 942, 941, -1, 943, 944, 941, -1, 897, 591, 590, -1, 336, 609, 572, -1, 609, 903, 572, -1, 580, 576, 619, -1, 898, 897, 588, -1, 897, 590, 588, -1, 576, 573, 902, -1, 619, 576, 902, -1, 903, 904, 577, -1, 572, 903, 577, -1, 902, 573, 901, -1, 895, 898, 585, -1, 898, 588, 585, -1, 901, 573, 570, -1, 894, 895, 584, -1, 901, 570, 900, -1, 895, 585, 584, -1, 900, 570, 569, -1, 904, 894, 581, -1, 894, 584, 581, -1, 577, 904, 581, -1, 900, 569, 899, -1, 896, 899, 594, -1, 899, 569, 594, -1, 897, 896, 591, -1, 896, 594, 591, -1, 664, 667, 945, -1, 651, 634, 371, -1, 642, 654, 368, -1, 651, 946, 634, -1, 947, 650, 654, -1, 662, 945, 948, -1, 662, 664, 945, -1, 947, 654, 642, -1, 655, 949, 946, -1, 950, 650, 947, -1, 655, 946, 651, -1, 644, 650, 950, -1, 661, 948, 951, -1, 661, 662, 948, -1, 952, 644, 950, -1, 658, 951, 953, -1, 647, 644, 952, -1, 658, 661, 951, -1, 657, 953, 949, -1, 657, 658, 953, -1, 954, 647, 952, -1, 657, 949, 655, -1, 668, 954, 955, -1, 668, 647, 954, -1, 667, 955, 945, -1, 667, 668, 955, -1, 681, 630, 637, -1, 956, 953, 951, -1, 957, 956, 951, -1, 690, 687, 641, -1, 687, 637, 641, -1, 957, 951, 948, -1, 958, 959, 955, -1, 959, 945, 955, -1, 960, 957, 948, -1, 960, 948, 959, -1, 671, 690, 622, -1, 690, 641, 622, -1, 958, 955, 961, -1, 959, 948, 945, -1, 961, 955, 954, -1, 670, 671, 621, -1, 671, 622, 621, -1, 961, 954, 962, -1, 962, 954, 952, -1, 672, 670, 623, -1, 670, 621, 623, -1, 962, 952, 963, -1, 672, 623, 674, -1, 963, 952, 950, -1, 674, 623, 625, -1, 963, 950, 964, -1, 676, 674, 627, -1, 964, 950, 947, -1, 674, 625, 627, -1, 964, 947, 691, -1, 677, 676, 629, -1, 676, 627, 629, -1, 691, 947, 642, -1, 689, 691, 639, -1, 682, 677, 634, -1, 677, 629, 634, -1, 691, 642, 639, -1, 682, 634, 965, -1, 685, 689, 635, -1, 965, 634, 946, -1, 689, 639, 635, -1, 965, 946, 966, -1, 681, 685, 630, -1, 966, 946, 949, -1, 685, 635, 630, -1, 966, 949, 953, -1, 956, 966, 953, -1, 687, 681, 637, -1, 959, 666, 665, -1, 380, 682, 648, -1, 682, 965, 648, -1, 389, 652, 691, -1, 960, 959, 663, -1, 959, 665, 663, -1, 652, 649, 964, -1, 691, 652, 964, -1, 965, 966, 653, -1, 648, 965, 653, -1, 964, 649, 963, -1, 957, 960, 660, -1, 960, 663, 660, -1, 963, 649, 646, -1, 956, 957, 659, -1, 963, 646, 962, -1, 957, 660, 659, -1, 962, 646, 645, -1, 966, 956, 656, -1, 956, 659, 656, -1, 653, 966, 656, -1, 962, 645, 961, -1, 958, 961, 669, -1, 961, 645, 669, -1, 959, 958, 666, -1, 958, 669, 666, -1, 939, 942, 967, -1, 873, 939, 967, -1, 968, 728, 969, -1, 968, 969, 970, -1, 971, 968, 970, -1, 969, 728, 972, -1, 728, 727, 972, -1, 971, 970, 973, -1, 974, 971, 973, -1, 972, 727, 975, -1, 727, 976, 975, -1, 974, 973, 977, -1, 978, 974, 977, -1, 975, 976, 979, -1, 976, 980, 979, -1, 978, 977, 981, -1, 733, 978, 981, -1, 979, 980, 982, -1, 980, 983, 982, -1, 733, 981, 984, -1, 985, 733, 984, -1, 982, 983, 986, -1, 983, 987, 986, -1, 985, 984, 988, -1, 735, 985, 988, -1, 989, 990, 991, -1, 987, 989, 991, -1, 986, 987, 991, -1, 735, 988, 992, -1, 736, 735, 992, -1, 991, 990, 993, -1, 990, 994, 993, -1, 736, 992, 995, -1, 737, 736, 995, -1, 994, 996, 997, -1, 993, 994, 997, -1, 998, 737, 999, -1, 737, 995, 999, -1, 996, 1000, 1001, -1, 997, 996, 1001, -1, 739, 998, 1002, -1, 998, 999, 1002, -1, 1001, 1000, 1003, -1, 1000, 1004, 1003, -1, 740, 739, 1005, -1, 739, 1002, 1005, -1, 1003, 1004, 1006, -1, 1004, 1007, 1006, -1, 741, 740, 1008, -1, 740, 1005, 1008, -1, 1006, 1007, 1009, -1, 1007, 1010, 1009, -1, 742, 741, 1011, -1, 741, 1008, 1011, -1, 1009, 1010, 1012, -1, 742, 1011, 1013, -1, 742, 1013, 743, -1, 743, 1013, 1014, -1, 1012, 1010, 1015, -1, 1016, 1012, 1015, -1, 743, 1014, 1017, -1, 1014, 1018, 1017, -1, 1019, 1016, 1020, -1, 1016, 1015, 1020, -1, 1017, 1018, 745, -1, 1018, 1021, 745, -1, 1019, 1020, 1022, -1, 1023, 1019, 1022, -1, 745, 1021, 1024, -1, 1021, 1025, 1024, -1, 1026, 1023, 1027, -1, 1023, 1022, 1027, -1, 1024, 1025, 1028, -1, 1025, 1029, 1028, -1, 1030, 1026, 1031, -1, 1026, 1027, 1031, -1, 1029, 1032, 1033, -1, 1028, 1029, 1033, -1, 1034, 1030, 1035, -1, 1030, 1031, 1035, -1, 1032, 1036, 1037, -1, 1033, 1032, 1037, -1, 1038, 1034, 1039, -1, 1034, 1035, 1039, -1, 1037, 1036, 1040, -1, 1041, 1038, 1042, -1, 1038, 1039, 1042, -1, 1036, 1043, 1044, -1, 1040, 1036, 1044, -1, 1041, 1042, 1045, -1, 1046, 1041, 1045, -1, 1043, 1047, 1048, -1, 1044, 1043, 1048, -1, 1049, 1046, 1050, -1, 1046, 1045, 1050, -1, 1047, 1051, 1052, -1, 1048, 1047, 1052, -1, 1053, 1049, 1054, -1, 1049, 1050, 1054, -1, 1051, 1055, 1056, -1, 1052, 1051, 1056, -1, 1057, 1053, 1058, -1, 1053, 1054, 1058, -1, 1055, 1059, 1060, -1, 1056, 1055, 1060, -1, 1061, 1057, 1062, -1, 1057, 1058, 1062, -1, 1059, 1063, 1064, -1, 1060, 1059, 1064, -1, 1065, 1061, 1066, -1, 1061, 1062, 1066, -1, 1065, 1066, 1067, -1, 1063, 1065, 1067, -1, 1064, 1063, 1067, -1, 1068, 1069, 750, -1, 1068, 750, 1070, -1, 1071, 1072, 1073, -1, 1074, 1071, 1073, -1, 750, 1071, 1074, -1, 1070, 750, 1074, -1, 1073, 1072, 1075, -1, 1073, 1075, 1076, -1, 1077, 1078, 1079, -1, 1080, 1077, 1079, -1, 1075, 1077, 1080, -1, 1076, 1075, 1080, -1, 1081, 1082, 1068, -1, 1082, 1069, 1068, -1, 1083, 1082, 1081, -1, 1084, 1083, 1081, -1, 1085, 1083, 1084, -1, 1085, 758, 1083, -1, 1086, 1087, 758, -1, 1086, 758, 1085, -1, 1088, 1089, 1087, -1, 1088, 1087, 1086, -1, 1078, 1089, 1088, -1, 1079, 1078, 1088, -1, 1090, 832, 833, -1, 1090, 833, 834, -1, 1090, 834, 835, -1, 1091, 832, 1090, -1, 1091, 830, 831, -1, 1091, 831, 832, -1, 1092, 1090, 835, -1, 1092, 835, 836, -1, 1092, 836, 837, -1, 1093, 830, 1091, -1, 1093, 820, 829, -1, 1093, 829, 830, -1, 1094, 1092, 837, -1, 1094, 837, 827, -1, 1094, 827, 826, -1, 1094, 826, 823, -1, 1095, 820, 1093, -1, 1095, 817, 818, -1, 1095, 818, 819, -1, 1095, 819, 820, -1, 1096, 1094, 823, -1, 1096, 823, 822, -1, 1096, 822, 824, -1, 1097, 817, 1095, -1, 1097, 814, 815, -1, 1097, 815, 817, -1, 1098, 1096, 824, -1, 1098, 824, 816, -1, 1098, 816, 802, -1, 1099, 812, 813, -1, 1099, 813, 814, -1, 1099, 814, 1097, -1, 1100, 1098, 802, -1, 1100, 802, 801, -1, 1100, 801, 803, -1, 1100, 803, 804, -1, 1101, 1100, 804, -1, 805, 1101, 804, -1, 806, 1102, 1101, -1, 806, 1101, 805, -1, 1103, 1104, 1105, -1, 1106, 1104, 1103, -1, 1106, 1107, 1104, -1, 1108, 1103, 1105, -1, 1109, 1107, 1106, -1, 1110, 1108, 1105, -1, 1111, 1110, 1105, -1, 1112, 1099, 1107, -1, 1112, 812, 1099, -1, 1112, 811, 812, -1, 1112, 1107, 1109, -1, 1113, 811, 1112, -1, 1113, 798, 799, -1, 1113, 799, 811, -1, 1114, 1115, 1111, -1, 807, 1102, 806, -1, 1116, 798, 1113, -1, 1117, 1115, 1114, -1, 797, 798, 1116, -1, 1118, 797, 1116, -1, 1119, 1114, 1120, -1, 1119, 1117, 1114, -1, 1121, 1120, 1122, -1, 1121, 1119, 1120, -1, 1123, 1121, 1122, -1, 808, 1102, 807, -1, 808, 1124, 1102, -1, 1125, 1122, 1126, -1, 1125, 1123, 1122, -1, 796, 797, 1118, -1, 809, 1124, 808, -1, 795, 1118, 1127, -1, 795, 796, 1118, -1, 792, 1124, 809, -1, 794, 1127, 1128, -1, 794, 795, 1127, -1, 791, 1129, 1124, -1, 791, 1124, 792, -1, 790, 1128, 1130, -1, 790, 794, 1128, -1, 789, 1129, 791, -1, 788, 790, 1130, -1, 788, 1130, 1131, -1, 787, 1132, 1129, -1, 787, 1129, 789, -1, 786, 788, 1131, -1, 785, 1132, 787, -1, 784, 1131, 1133, -1, 784, 786, 1131, -1, 783, 1132, 785, -1, 782, 1133, 1134, -1, 782, 784, 1133, -1, 781, 1126, 1132, -1, 781, 1132, 783, -1, 780, 1134, 1135, -1, 780, 782, 1134, -1, 779, 1126, 781, -1, 778, 780, 1135, -1, 777, 1126, 779, -1, 777, 1125, 1126, -1, 773, 778, 1135, -1, 773, 1135, 1136, -1, 776, 1125, 777, -1, 776, 773, 1136, -1, 776, 1136, 1125, -1, 840, 839, 1137, -1, 1137, 839, 843, -1, 1138, 905, 838, -1, 1138, 838, 840, -1, 1139, 1140, 1137, -1, 1139, 1137, 843, -1, 1139, 1141, 1140, -1, 1139, 843, 841, -1, 1139, 841, 1141, -1, 845, 850, 1142, -1, 845, 1142, 846, -1, 1143, 846, 1144, -1, 1142, 1145, 1146, -1, 1145, 1147, 1146, -1, 1147, 1144, 1146, -1, 846, 1142, 1146, -1, 1144, 846, 1146, -1, 1148, 1149, 846, -1, 1150, 846, 1149, -1, 1151, 1148, 846, -1, 1152, 846, 1150, -1, 1153, 846, 1143, -1, 1153, 1151, 846, -1, 1154, 1153, 1143, -1, 1155, 846, 1152, -1, 1156, 1154, 1143, -1, 847, 846, 1157, -1, 1157, 846, 1155, -1, 1158, 841, 1159, -1, 1160, 1141, 841, -1, 1160, 841, 1158, -1, 841, 847, 1159, -1, 850, 849, 1161, -1, 1161, 849, 853, -1, 1162, 859, 861, -1, 1162, 857, 859, -1, 1162, 858, 857, -1, 1162, 1163, 858, -1, 1162, 861, 1163, -1, 858, 1164, 855, -1, 1165, 1164, 858, -1, 853, 854, 1166, -1, 854, 856, 1166, -1, 856, 855, 1166, -1, 855, 1161, 1166, -1, 1161, 853, 1166, -1, 863, 865, 1167, -1, 1168, 872, 1169, -1, 1167, 865, 872, -1, 1168, 1170, 1171, -1, 1170, 1172, 1171, -1, 1172, 1167, 1171, -1, 1167, 872, 1171, -1, 872, 1168, 1171, -1, 1163, 864, 863, -1, 861, 864, 1163, -1, 1173, 869, 1174, -1, 1174, 1175, 1176, -1, 1173, 871, 869, -1, 1174, 869, 1175, -1, 872, 1177, 1178, -1, 1177, 872, 1179, -1, 1169, 872, 1180, -1, 872, 1178, 1180, -1, 1179, 872, 1181, -1, 1169, 1180, 1182, -1, 1181, 872, 1183, -1, 1169, 1182, 1184, -1, 1185, 872, 871, -1, 1183, 872, 1185, -1, 1186, 873, 967, -1, 874, 873, 1186, -1, 1186, 1187, 1188, -1, 874, 1186, 1188, -1, 1187, 1175, 1188, -1, 869, 874, 1188, -1, 1175, 869, 1188, -1, 907, 905, 915, -1, 908, 907, 915, -1, 1189, 1190, 1191, -1, 1189, 905, 1190, -1, 916, 1191, 917, -1, 1192, 916, 1193, -1, 919, 1192, 1193, -1, 916, 920, 1193, -1, 920, 919, 1193, -1, 918, 1194, 917, -1, 922, 918, 1195, -1, 1196, 922, 1195, -1, 1197, 924, 922, -1, 1197, 922, 1196, -1, 929, 924, 1197, -1, 1198, 929, 1197, -1, 1199, 929, 930, -1, 928, 930, 1200, -1, 926, 927, 1201, -1, 1201, 927, 928, -1, 943, 940, 937, -1, 943, 937, 944, -1, 1200, 1202, 1203, -1, 1202, 944, 1203, -1, 942, 944, 1204, -1, 967, 942, 1204, -1, 969, 972, 1205, -1, 970, 969, 1205, -1, 973, 970, 1205, -1, 1205, 972, 1206, -1, 975, 979, 1206, -1, 972, 975, 1206, -1, 977, 973, 1207, -1, 981, 977, 1207, -1, 973, 1205, 1207, -1, 1206, 979, 1208, -1, 982, 986, 1208, -1, 979, 982, 1208, -1, 984, 981, 1209, -1, 988, 984, 1209, -1, 981, 1207, 1209, -1, 1208, 986, 1210, -1, 993, 997, 1210, -1, 991, 993, 1210, -1, 986, 991, 1210, -1, 992, 988, 1211, -1, 995, 992, 1211, -1, 988, 1209, 1211, -1, 1001, 1003, 1212, -1, 997, 1001, 1212, -1, 1210, 997, 1212, -1, 995, 1211, 1213, -1, 999, 995, 1213, -1, 1002, 999, 1213, -1, 1005, 1002, 1213, -1, 1212, 1003, 1214, -1, 1006, 1009, 1214, -1, 1003, 1006, 1214, -1, 1005, 1213, 1215, -1, 1008, 1005, 1215, -1, 1008, 1215, 1011, -1, 1009, 1012, 1216, -1, 1214, 1009, 1216, -1, 1011, 1215, 1217, -1, 1013, 1011, 1217, -1, 1216, 1012, 1016, -1, 1013, 1217, 1014, -1, 1014, 1217, 1218, -1, 1219, 1216, 1019, -1, 1216, 1016, 1019, -1, 1014, 1218, 1018, -1, 1219, 1019, 1023, -1, 1018, 1218, 1021, -1, 1220, 1219, 1026, -1, 1219, 1023, 1026, -1, 1218, 1221, 1025, -1, 1021, 1218, 1025, -1, 1220, 1026, 1030, -1, 1025, 1221, 1029, -1, 1222, 1220, 1034, -1, 1220, 1030, 1034, -1, 1029, 1221, 1032, -1, 1221, 1223, 1032, -1, 1222, 1034, 1038, -1, 1032, 1223, 1036, -1, 1224, 1222, 1041, -1, 1222, 1038, 1041, -1, 1036, 1223, 1043, -1, 1223, 1225, 1043, -1, 1224, 1041, 1046, -1, 1043, 1225, 1047, -1, 1224, 1046, 1049, -1, 1047, 1225, 1051, -1, 1225, 1226, 1051, -1, 1227, 1224, 1053, -1, 1224, 1049, 1053, -1, 1051, 1226, 1055, -1, 1227, 1053, 1057, -1, 1055, 1226, 1059, -1, 1228, 1227, 1061, -1, 1227, 1057, 1061, -1, 1226, 1228, 1063, -1, 1059, 1226, 1063, -1, 1228, 1061, 1065, -1, 1063, 1228, 1065, -1, 1229, 1068, 1070, -1, 1229, 1070, 1230, -1, 1074, 1073, 1231, -1, 1232, 1074, 1231, -1, 1070, 1074, 1232, -1, 1230, 1070, 1232, -1, 1231, 1073, 1076, -1, 1231, 1076, 1233, -1, 1080, 1079, 1234, -1, 1235, 1080, 1234, -1, 1076, 1080, 1235, -1, 1233, 1076, 1235, -1, 1236, 1081, 1229, -1, 1081, 1068, 1229, -1, 1237, 1084, 1081, -1, 1237, 1081, 1236, -1, 1238, 1084, 1237, -1, 1238, 1085, 1084, -1, 1239, 1086, 1085, -1, 1239, 1085, 1238, -1, 1240, 1088, 1086, -1, 1240, 1086, 1239, -1, 1079, 1088, 1240, -1, 1234, 1079, 1240, -1, 1094, 1241, 1092, -1, 1242, 1241, 1094, -1, 1243, 1244, 1111, -1, 1096, 1242, 1094, -1, 1245, 1242, 1096, -1, 1105, 1243, 1111, -1, 1246, 1096, 1098, -1, 1247, 1243, 1105, -1, 1246, 1245, 1096, -1, 1104, 1247, 1105, -1, 1248, 1098, 1100, -1, 1249, 1247, 1104, -1, 1248, 1246, 1098, -1, 1107, 1249, 1104, -1, 1250, 1249, 1107, -1, 1251, 1100, 1101, -1, 1251, 1248, 1100, -1, 1099, 1250, 1107, -1, 1252, 1250, 1099, -1, 1253, 1251, 1101, -1, 1253, 1101, 1102, -1, 1097, 1252, 1099, -1, 1254, 1253, 1102, -1, 1255, 1097, 1095, -1, 1254, 1102, 1124, -1, 1255, 1252, 1097, -1, 1256, 1254, 1124, -1, 1093, 1255, 1095, -1, 1256, 1124, 1129, -1, 1257, 1255, 1093, -1, 1258, 1093, 1091, -1, 1258, 1257, 1093, -1, 1259, 1091, 1090, -1, 1259, 1258, 1091, -1, 1241, 1090, 1092, -1, 1241, 1259, 1090, -1, 1132, 1256, 1129, -1, 1260, 1256, 1132, -1, 1126, 1260, 1132, -1, 1122, 1261, 1126, -1, 1120, 1262, 1122, -1, 1114, 1263, 1120, -1, 1264, 1263, 1114, -1, 1111, 1264, 1114, -1, 1244, 1264, 1111, -1, 1261, 1260, 1126, -1, 1262, 1261, 1122, -1, 1263, 1262, 1120, -1, 1128, 1265, 1266, -1, 1128, 1127, 1265, -1, 1267, 1111, 1244, -1, 1130, 1266, 1268, -1, 1130, 1128, 1266, -1, 1110, 1111, 1267, -1, 1269, 1110, 1267, -1, 1131, 1268, 1270, -1, 1131, 1130, 1268, -1, 1108, 1110, 1269, -1, 1271, 1108, 1269, -1, 1133, 1270, 1272, -1, 1133, 1131, 1270, -1, 1103, 1108, 1271, -1, 1273, 1103, 1271, -1, 1134, 1272, 1274, -1, 1106, 1103, 1273, -1, 1134, 1133, 1272, -1, 1275, 1106, 1273, -1, 1135, 1134, 1274, -1, 1135, 1274, 1276, -1, 1109, 1106, 1275, -1, 1277, 1109, 1275, -1, 1136, 1135, 1276, -1, 1136, 1276, 1278, -1, 1112, 1109, 1277, -1, 1113, 1277, 1279, -1, 1125, 1136, 1278, -1, 1125, 1278, 1280, -1, 1113, 1112, 1277, -1, 1116, 1279, 1281, -1, 1116, 1113, 1279, -1, 1118, 1281, 1282, -1, 1118, 1116, 1281, -1, 1127, 1282, 1265, -1, 1127, 1118, 1282, -1, 1283, 1125, 1280, -1, 1123, 1125, 1283, -1, 1284, 1123, 1283, -1, 1121, 1123, 1284, -1, 1285, 1121, 1284, -1, 1119, 1121, 1285, -1, 1286, 1119, 1285, -1, 1117, 1119, 1286, -1, 1287, 1117, 1286, -1, 1115, 1117, 1287, -1, 1244, 1115, 1287, -1, 1111, 1115, 1244, -1, 1138, 840, 1137, -1, 1140, 1138, 1137, -1, 1190, 905, 1138, -1, 1138, 1140, 1288, -1, 1289, 1190, 1288, -1, 1140, 1289, 1288, -1, 1190, 1138, 1288, -1, 1290, 1141, 1160, -1, 1141, 1290, 1291, -1, 1140, 1141, 1291, -1, 1145, 1142, 1147, -1, 850, 1161, 1292, -1, 1142, 850, 1292, -1, 1161, 1147, 1292, -1, 1147, 1142, 1292, -1, 1144, 1293, 1294, -1, 1144, 1147, 1293, -1, 1144, 1295, 1143, -1, 1294, 1295, 1144, -1, 1296, 1297, 1298, -1, 1296, 1294, 1297, -1, 1296, 1298, 1295, -1, 1296, 1295, 1294, -1, 1140, 1299, 1157, -1, 1300, 1140, 1157, -1, 1299, 847, 1157, -1, 1301, 1300, 1155, -1, 1300, 1157, 1155, -1, 1302, 1301, 1152, -1, 1301, 1155, 1152, -1, 1303, 1302, 1150, -1, 1302, 1152, 1150, -1, 1304, 1303, 1149, -1, 1303, 1150, 1149, -1, 1304, 1149, 1148, -1, 1304, 1148, 1305, -1, 1305, 1148, 1151, -1, 1305, 1151, 1306, -1, 1306, 1151, 1153, -1, 1306, 1153, 1307, -1, 1308, 1307, 1154, -1, 1307, 1153, 1154, -1, 1309, 1308, 1156, -1, 1308, 1154, 1156, -1, 1298, 1310, 1311, -1, 1312, 1310, 1309, -1, 1312, 1143, 1311, -1, 1312, 1156, 1143, -1, 1312, 1309, 1156, -1, 1312, 1311, 1310, -1, 1290, 1160, 1158, -1, 1291, 1290, 1159, -1, 1290, 1158, 1159, -1, 1140, 1291, 1299, -1, 1291, 1159, 1299, -1, 1299, 1159, 847, -1, 858, 1163, 1170, -1, 1313, 1314, 1196, -1, 1315, 1316, 1164, -1, 1165, 1315, 1164, -1, 1317, 1197, 1318, -1, 1319, 1197, 1196, -1, 1319, 1315, 1318, -1, 1319, 1314, 1316, -1, 1319, 1196, 1314, -1, 1319, 1316, 1315, -1, 1319, 1318, 1197, -1, 858, 1170, 1165, -1, 1170, 1320, 1165, -1, 855, 1293, 1147, -1, 1164, 1293, 855, -1, 1147, 1161, 855, -1, 1321, 1168, 1169, -1, 1322, 1168, 1321, -1, 1323, 1324, 1322, -1, 1323, 1325, 1324, -1, 1323, 1321, 1325, -1, 1323, 1322, 1321, -1, 1320, 1170, 1168, -1, 1322, 1320, 1168, -1, 1170, 1167, 1172, -1, 1326, 1163, 863, -1, 1326, 1170, 1163, -1, 1326, 863, 1167, -1, 1326, 1167, 1170, -1, 1176, 1175, 1327, -1, 1175, 1187, 1328, -1, 1175, 1328, 1327, -1, 1174, 1176, 1327, -1, 1174, 1327, 1173, -1, 1173, 1327, 1328, -1, 871, 1173, 1329, -1, 1173, 1328, 1329, -1, 1329, 1328, 1187, -1, 871, 1329, 1330, -1, 1185, 871, 1330, -1, 1329, 1187, 1330, -1, 1185, 1330, 1331, -1, 1185, 1331, 1183, -1, 1183, 1331, 1332, -1, 1183, 1332, 1181, -1, 1179, 1181, 1333, -1, 1181, 1332, 1333, -1, 1177, 1179, 1334, -1, 1179, 1333, 1334, -1, 1178, 1177, 1335, -1, 1177, 1334, 1335, -1, 1180, 1178, 1336, -1, 1178, 1335, 1336, -1, 1182, 1180, 1337, -1, 1180, 1336, 1337, -1, 1184, 1182, 1338, -1, 1182, 1337, 1338, -1, 1339, 1340, 1325, -1, 1184, 1338, 1341, -1, 1338, 1340, 1341, -1, 1339, 1169, 1341, -1, 1169, 1184, 1341, -1, 1340, 1339, 1341, -1, 1186, 967, 1204, -1, 1186, 1204, 1187, -1, 1342, 1191, 1190, -1, 1289, 1342, 1190, -1, 1343, 1344, 1289, -1, 1345, 1344, 1346, -1, 1345, 1346, 1342, -1, 1345, 1342, 1289, -1, 1345, 1289, 1344, -1, 1342, 917, 1191, -1, 1347, 917, 1342, -1, 1194, 917, 1347, -1, 1347, 1348, 1194, -1, 1346, 1344, 1349, -1, 1347, 1346, 1349, -1, 1344, 1348, 1349, -1, 1348, 1347, 1349, -1, 1195, 918, 1194, -1, 1350, 1351, 1348, -1, 1194, 1348, 1195, -1, 1352, 1196, 1195, -1, 1352, 1313, 1196, -1, 1352, 1351, 1313, -1, 1352, 1348, 1351, -1, 1352, 1195, 1348, -1, 1198, 1199, 929, -1, 1353, 1354, 1355, -1, 1353, 1199, 1198, -1, 1356, 1317, 1354, -1, 1356, 1197, 1317, -1, 1356, 1198, 1197, -1, 1356, 1353, 1198, -1, 1356, 1354, 1353, -1, 1357, 930, 1199, -1, 1199, 1353, 1357, -1, 1358, 1353, 1359, -1, 1358, 1359, 1360, -1, 1358, 1360, 1357, -1, 1358, 1357, 1353, -1, 1200, 930, 1361, -1, 1361, 930, 1357, -1, 1204, 944, 1202, -1, 1362, 1187, 1204, -1, 1362, 1202, 1363, -1, 1362, 1363, 1187, -1, 1362, 1204, 1202, -1, 1202, 1200, 1361, -1, 1202, 1361, 1363, -1, 1363, 1359, 1364, -1, 1360, 1359, 1365, -1, 1361, 1360, 1365, -1, 1363, 1361, 1365, -1, 1359, 1363, 1365, -1, 1366, 1367, 1223, -1, 1366, 1223, 1221, -1, 1368, 1366, 1221, -1, 1218, 1368, 1221, -1, 1369, 1368, 1218, -1, 1217, 1369, 1218, -1, 1370, 1369, 1217, -1, 1215, 1370, 1217, -1, 1371, 1370, 1215, -1, 1213, 1371, 1215, -1, 1372, 1371, 1213, -1, 1211, 1372, 1213, -1, 1373, 1372, 1211, -1, 1209, 1373, 1211, -1, 1374, 1373, 1209, -1, 1207, 1374, 1209, -1, 1375, 1374, 1207, -1, 1205, 1375, 1207, -1, 1376, 1375, 1205, -1, 1206, 1376, 1205, -1, 1377, 1376, 1206, -1, 1208, 1377, 1206, -1, 1378, 1377, 1208, -1, 1210, 1378, 1208, -1, 1379, 1378, 1210, -1, 1212, 1379, 1210, -1, 1380, 1379, 1212, -1, 1214, 1380, 1212, -1, 1381, 1380, 1214, -1, 1216, 1381, 1214, -1, 1382, 1381, 1216, -1, 1219, 1382, 1216, -1, 1383, 1382, 1219, -1, 1220, 1383, 1219, -1, 1384, 1383, 1220, -1, 1222, 1384, 1220, -1, 1385, 1384, 1222, -1, 1224, 1385, 1222, -1, 1386, 1385, 1224, -1, 1227, 1386, 1224, -1, 1387, 1386, 1227, -1, 1228, 1387, 1227, -1, 1388, 1387, 1228, -1, 1226, 1388, 1228, -1, 1389, 1388, 1226, -1, 1225, 1389, 1226, -1, 1390, 1389, 1225, -1, 1223, 1390, 1225, -1, 1367, 1390, 1223, -1, 1391, 1392, 1230, -1, 1391, 1230, 1393, -1, 1394, 1391, 1395, -1, 1394, 1395, 1396, -1, 1238, 1397, 1392, -1, 1398, 1397, 1238, -1, 1399, 1400, 1238, -1, 1399, 1238, 1394, -1, 1284, 1283, 1401, -1, 1279, 1402, 1281, -1, 1403, 1284, 1401, -1, 1264, 1287, 1286, -1, 1249, 1404, 1247, -1, 1264, 1286, 1263, -1, 1281, 1402, 1405, -1, 1266, 1406, 1407, -1, 1268, 1266, 1407, -1, 1281, 1405, 1282, -1, 1263, 1286, 1285, -1, 1404, 1408, 1273, -1, 1247, 1404, 1273, -1, 1271, 1247, 1273, -1, 1401, 1283, 1409, -1, 1243, 1247, 1269, -1, 1280, 1278, 1409, -1, 1283, 1280, 1409, -1, 1247, 1271, 1269, -1, 1268, 1407, 1410, -1, 1263, 1285, 1284, -1, 1408, 1411, 1275, -1, 1270, 1268, 1410, -1, 1262, 1263, 1403, -1, 1273, 1408, 1275, -1, 1263, 1284, 1403, -1, 1409, 1278, 1412, -1, 1265, 1282, 1413, -1, 1278, 1276, 1412, -1, 1282, 1405, 1413, -1, 1243, 1269, 1267, -1, 1411, 1414, 1277, -1, 1270, 1410, 1415, -1, 1272, 1270, 1415, -1, 1275, 1411, 1277, -1, 1276, 1274, 1416, -1, 1412, 1276, 1416, -1, 1243, 1267, 1244, -1, 1416, 1274, 1417, -1, 1274, 1272, 1417, -1, 1272, 1415, 1417, -1, 1414, 1402, 1279, -1, 1277, 1414, 1279, -1, 1265, 1413, 1406, -1, 1244, 1287, 1264, -1, 1266, 1265, 1406, -1, 1418, 1419, 1259, -1, 1259, 1419, 1258, -1, 1420, 1421, 1258, -1, 1419, 1420, 1258, -1, 1422, 1418, 1241, -1, 1423, 1422, 1241, -1, 1418, 1259, 1241, -1, 1258, 1421, 1257, -1, 1421, 1424, 1257, -1, 1423, 1241, 1242, -1, 1425, 1423, 1242, -1, 1257, 1424, 1255, -1, 1426, 1427, 1255, -1, 1424, 1426, 1255, -1, 1425, 1242, 1245, -1, 1428, 1425, 1245, -1, 1428, 1245, 1246, -1, 1429, 1428, 1246, -1, 1430, 1429, 1246, -1, 1255, 1427, 1252, -1, 1431, 1432, 1252, -1, 1427, 1431, 1252, -1, 1430, 1246, 1248, -1, 1433, 1430, 1248, -1, 1433, 1248, 1251, -1, 1434, 1433, 1251, -1, 1249, 1250, 1404, -1, 1250, 1252, 1408, -1, 1404, 1250, 1408, -1, 1252, 1432, 1408, -1, 1432, 1435, 1408, -1, 1434, 1251, 1253, -1, 1436, 1434, 1253, -1, 1437, 1438, 1411, -1, 1435, 1437, 1411, -1, 1408, 1435, 1411, -1, 1439, 1440, 1414, -1, 1441, 1439, 1414, -1, 1438, 1441, 1414, -1, 1411, 1438, 1414, -1, 1253, 1254, 1442, -1, 1436, 1253, 1442, -1, 1443, 1444, 1402, -1, 1440, 1443, 1402, -1, 1414, 1440, 1402, -1, 1444, 1445, 1405, -1, 1402, 1444, 1405, -1, 1442, 1254, 1446, -1, 1405, 1445, 1447, -1, 1261, 1262, 1403, -1, 1405, 1447, 1413, -1, 1260, 1261, 1401, -1, 1261, 1403, 1401, -1, 1254, 1256, 1448, -1, 1446, 1254, 1448, -1, 1413, 1447, 1449, -1, 1448, 1256, 1450, -1, 1413, 1449, 1451, -1, 1256, 1260, 1452, -1, 1450, 1256, 1452, -1, 1413, 1451, 1453, -1, 1406, 1413, 1453, -1, 1452, 1260, 1454, -1, 1260, 1401, 1454, -1, 1406, 1453, 1455, -1, 1454, 1401, 1456, -1, 1407, 1406, 1457, -1, 1406, 1455, 1457, -1, 1401, 1409, 1458, -1, 1456, 1401, 1458, -1, 1407, 1457, 1459, -1, 1458, 1409, 1460, -1, 1407, 1459, 1461, -1, 1409, 1412, 1462, -1, 1460, 1409, 1462, -1, 1410, 1407, 1463, -1, 1407, 1461, 1463, -1, 1462, 1412, 1464, -1, 1410, 1463, 1465, -1, 1464, 1412, 1466, -1, 1415, 1410, 1467, -1, 1410, 1465, 1467, -1, 1412, 1416, 1468, -1, 1466, 1412, 1468, -1, 1415, 1467, 1469, -1, 1468, 1416, 1470, -1, 1415, 1469, 1471, -1, 1416, 1417, 1472, -1, 1470, 1416, 1472, -1, 1417, 1415, 1473, -1, 1415, 1471, 1473, -1, 1417, 1473, 1474, -1, 1472, 1417, 1474, -1, 1289, 1304, 1305, -1, 1304, 1289, 1303, -1, 1289, 1305, 1306, -1, 1303, 1289, 1302, -1, 1310, 1289, 1307, -1, 1289, 1306, 1307, -1, 1310, 1307, 1308, -1, 1302, 1289, 1301, -1, 1310, 1308, 1309, -1, 1289, 1140, 1300, -1, 1301, 1289, 1300, -1, 1475, 1350, 1348, -1, 1476, 1477, 1475, -1, 1476, 1293, 1477, -1, 1476, 1294, 1293, -1, 1476, 1297, 1294, -1, 1476, 1348, 1297, -1, 1476, 1475, 1348, -1, 1297, 1343, 1298, -1, 1297, 1348, 1478, -1, 1348, 1344, 1478, -1, 1344, 1343, 1478, -1, 1343, 1297, 1478, -1, 1295, 1298, 1311, -1, 1295, 1311, 1143, -1, 1298, 1289, 1310, -1, 1298, 1343, 1289, -1, 1317, 1318, 1479, -1, 1318, 1315, 1479, -1, 1315, 1165, 1479, -1, 1165, 1480, 1479, -1, 1480, 1317, 1479, -1, 1481, 1314, 1313, -1, 1481, 1316, 1314, -1, 1481, 1164, 1316, -1, 1481, 1482, 1164, -1, 1481, 1313, 1482, -1, 1320, 1480, 1165, -1, 1164, 1482, 1293, -1, 1483, 1353, 1355, -1, 1484, 1324, 1353, -1, 1484, 1322, 1324, -1, 1484, 1320, 1322, -1, 1484, 1485, 1320, -1, 1484, 1483, 1485, -1, 1484, 1353, 1483, -1, 1325, 1364, 1324, -1, 1486, 1353, 1324, -1, 1486, 1359, 1353, -1, 1486, 1364, 1359, -1, 1486, 1324, 1364, -1, 1487, 1321, 1169, -1, 1487, 1325, 1321, -1, 1487, 1339, 1325, -1, 1487, 1169, 1339, -1, 1335, 1334, 1363, -1, 1333, 1363, 1334, -1, 1336, 1363, 1340, -1, 1336, 1335, 1363, -1, 1332, 1363, 1333, -1, 1337, 1336, 1340, -1, 1331, 1363, 1332, -1, 1338, 1337, 1340, -1, 1187, 1363, 1330, -1, 1330, 1363, 1331, -1, 1340, 1363, 1325, -1, 1363, 1364, 1325, -1, 1347, 1342, 1346, -1, 1482, 1351, 1350, -1, 1313, 1351, 1482, -1, 1355, 1354, 1480, -1, 1480, 1354, 1317, -1, 1361, 1357, 1360, -1, 1488, 1489, 1490, -1, 1488, 1490, 1491, -1, 1488, 1491, 1492, -1, 1488, 1492, 1493, -1, 1488, 1493, 1494, -1, 1488, 1494, 1495, -1, 1488, 1495, 1496, -1, 1488, 1496, 1497, -1, 1488, 1497, 1498, -1, 1488, 1498, 1499, -1, 1488, 1499, 1500, -1, 1488, 1500, 1501, -1, 1488, 1501, 1502, -1, 1488, 1502, 1503, -1, 1488, 1503, 1504, -1, 1488, 1504, 1505, -1, 1488, 1505, 1506, -1, 1488, 1506, 1507, -1, 1488, 1507, 1508, -1, 1488, 1508, 1509, -1, 1488, 1509, 1510, -1, 1488, 1510, 1511, -1, 1488, 1511, 1512, -1, 1488, 1512, 1513, -1, 1488, 1513, 1489, -1, 1514, 1515, 1516, -1, 1517, 1516, 1515, -1, 1518, 1515, 1514, -1, 1519, 1517, 1515, -1, 1520, 1515, 1518, -1, 1521, 1519, 1515, -1, 1522, 1515, 1520, -1, 1523, 1521, 1515, -1, 1524, 1515, 1522, -1, 1525, 1523, 1515, -1, 1526, 1515, 1524, -1, 1527, 1525, 1515, -1, 1528, 1515, 1526, -1, 1529, 1527, 1515, -1, 1530, 1531, 1529, -1, 1530, 1532, 1531, -1, 1530, 1533, 1532, -1, 1530, 1534, 1533, -1, 1530, 1535, 1534, -1, 1530, 1536, 1535, -1, 1530, 1537, 1536, -1, 1530, 1529, 1515, -1, 1538, 1539, 1540, -1, 1538, 1541, 1539, -1, 1538, 1542, 1541, -1, 1538, 1543, 1542, -1, 1538, 1544, 1543, -1, 1538, 1545, 1544, -1, 1538, 1528, 1545, -1, 1538, 1515, 1528, -1, 1546, 1538, 1540, -1, 1547, 1537, 1530, -1, 1548, 1538, 1546, -1, 1549, 1547, 1530, -1, 1550, 1538, 1548, -1, 1551, 1549, 1530, -1, 1552, 1538, 1550, -1, 1553, 1551, 1530, -1, 1554, 1538, 1552, -1, 1555, 1553, 1530, -1, 1556, 1538, 1554, -1, 1557, 1555, 1530, -1, 1558, 1557, 1530, -1, 1559, 1560, 1558, -1, 1559, 1561, 1560, -1, 1559, 1562, 1561, -1, 1559, 1563, 1562, -1, 1559, 1564, 1563, -1, 1559, 1565, 1564, -1, 1559, 1566, 1565, -1, 1559, 1567, 1566, -1, 1559, 1568, 1567, -1, 1559, 1569, 1568, -1, 1559, 1570, 1569, -1, 1559, 1571, 1570, -1, 1559, 1572, 1571, -1, 1559, 1558, 1530, -1, 1573, 1572, 1538, -1, 1573, 1538, 1574, -1, 1574, 1538, 1556, -1, 1538, 1572, 1559, -1, 1575, 1576, 1577, -1, 1578, 1576, 1575, -1, 1579, 1578, 1575, -1, 1580, 1578, 1579, -1, 1581, 1580, 1579, -1, 1582, 1580, 1581, -1, 1583, 1582, 1581, -1, 1584, 1582, 1583, -1, 1585, 1584, 1583, -1, 1586, 1584, 1585, -1, 1587, 1586, 1585, -1, 1588, 1586, 1587, -1, 1589, 1588, 1587, -1, 1590, 1588, 1589, -1, 1591, 1590, 1589, -1, 1592, 1590, 1591, -1, 1593, 1592, 1591, -1, 1594, 1592, 1593, -1, 1595, 1594, 1593, -1, 1596, 1594, 1595, -1, 1597, 1596, 1595, -1, 1598, 1596, 1597, -1, 1599, 1598, 1597, -1, 1600, 1598, 1599, -1, 1601, 1600, 1599, -1, 1602, 1600, 1601, -1, 1603, 1602, 1601, -1, 1604, 1602, 1603, -1, 1605, 1604, 1603, -1, 1606, 1604, 1605, -1, 1607, 1606, 1605, -1, 1608, 1606, 1607, -1, 1609, 1608, 1607, -1, 1610, 1608, 1609, -1, 1611, 1610, 1609, -1, 1612, 1610, 1611, -1, 1613, 1612, 1611, -1, 1614, 1612, 1613, -1, 1615, 1614, 1613, -1, 1616, 1614, 1615, -1, 1617, 1616, 1615, -1, 1618, 1616, 1617, -1, 1619, 1618, 1617, -1, 1620, 1618, 1619, -1, 1621, 1620, 1619, -1, 1622, 1620, 1621, -1, 1623, 1622, 1621, -1, 1624, 1622, 1623, -1, 1625, 1624, 1623, -1, 1626, 1624, 1625, -1, 1627, 1626, 1625, -1, 1628, 1626, 1627, -1, 1629, 1628, 1627, -1, 1630, 1628, 1629, -1, 1631, 1630, 1629, -1, 1632, 1630, 1631, -1, 1633, 1632, 1631, -1, 1634, 1632, 1633, -1, 1635, 1634, 1633, -1, 1636, 1634, 1635, -1, 1637, 1636, 1635, -1, 1638, 1636, 1637, -1, 1639, 1638, 1637, -1, 1640, 1638, 1639, -1, 1641, 1640, 1639, -1, 1642, 1640, 1641, -1, 1643, 1642, 1641, -1, 1644, 1642, 1643, -1, 1645, 1644, 1643, -1, 1646, 1644, 1645, -1, 1647, 1646, 1645, -1, 1648, 1646, 1647, -1, 1649, 1648, 1647, -1, 1650, 1648, 1649, -1, 1651, 1650, 1649, -1, 1652, 1650, 1651, -1, 1653, 1652, 1651, -1, 1654, 1652, 1653, -1, 1655, 1654, 1653, -1, 1656, 1654, 1655, -1, 1657, 1656, 1655, -1, 1658, 1656, 1657, -1, 1659, 1658, 1657, -1, 1660, 1658, 1659, -1, 1661, 1660, 1659, -1, 1662, 1660, 1661, -1, 1663, 1662, 1661, -1, 1664, 1662, 1663, -1, 1665, 1664, 1663, -1, 1666, 1664, 1665, -1, 1667, 1666, 1665, -1, 1668, 1666, 1667, -1, 1669, 1668, 1667, -1, 1670, 1668, 1669, -1, 1671, 1670, 1669, -1, 1672, 1670, 1671, -1, 1673, 1672, 1671, -1, 1674, 1672, 1673, -1, 1675, 1674, 1673, -1, 1676, 1674, 1675, -1, 1677, 1676, 1675, -1, 1678, 1676, 1677, -1, 1678, 1677, 1679, -1, 1680, 1678, 1679, -1, 1680, 1679, 1681, -1, 1682, 1680, 1681, -1, 1682, 1681, 1683, -1, 1684, 1682, 1683, -1, 1684, 1683, 1685, -1, 1686, 1684, 1685, -1, 1686, 1685, 1687, -1, 1576, 1686, 1687, -1, 1576, 1687, 1577, -1, 1293, 1475, 1477, -1, 1688, 1482, 1350, -1, 1688, 1293, 1482, -1, 1688, 1350, 1475, -1, 1688, 1475, 1293, -1, 1485, 1483, 1320, -1, 1355, 1480, 1689, -1, 1480, 1320, 1689, -1, 1483, 1355, 1689, -1, 1320, 1483, 1689, -1, 1690, 1514, 1516, -1, 1690, 1516, 1517, -1, 1691, 1522, 1520, -1, 1691, 1520, 1518, -1, 1691, 1518, 1514, -1, 1691, 1514, 1690, -1, 1692, 1517, 1519, -1, 1692, 1519, 1521, -1, 1692, 1690, 1517, -1, 1693, 1522, 1691, -1, 1693, 1526, 1524, -1, 1693, 1524, 1522, -1, 1694, 1521, 1523, -1, 1694, 1523, 1525, -1, 1694, 1525, 1527, -1, 1694, 1692, 1521, -1, 1695, 1529, 1531, -1, 1695, 1694, 1527, -1, 1695, 1527, 1529, -1, 1696, 1526, 1693, -1, 1696, 1545, 1528, -1, 1696, 1528, 1526, -1, 1697, 1545, 1696, -1, 1697, 1542, 1543, -1, 1697, 1543, 1544, -1, 1697, 1544, 1545, -1, 1698, 1531, 1532, -1, 1698, 1532, 1533, -1, 1698, 1533, 1534, -1, 1698, 1695, 1531, -1, 1699, 1534, 1535, -1, 1699, 1698, 1534, -1, 1699, 1535, 1536, -1, 1700, 1542, 1697, -1, 1700, 1539, 1541, -1, 1700, 1541, 1542, -1, 1701, 1536, 1537, -1, 1701, 1699, 1536, -1, 1702, 1539, 1700, -1, 1702, 1540, 1539, -1, 1546, 1540, 1702, -1, 1547, 1703, 1701, -1, 1547, 1701, 1537, -1, 1548, 1702, 1704, -1, 1548, 1546, 1702, -1, 1549, 1703, 1547, -1, 1550, 1548, 1704, -1, 1551, 1703, 1549, -1, 1552, 1704, 1705, -1, 1552, 1550, 1704, -1, 1553, 1706, 1703, -1, 1553, 1703, 1551, -1, 1554, 1552, 1705, -1, 1555, 1706, 1553, -1, 1556, 1554, 1705, -1, 1556, 1705, 1707, -1, 1557, 1706, 1555, -1, 1557, 1708, 1706, -1, 1574, 1556, 1707, -1, 1558, 1708, 1557, -1, 1573, 1574, 1707, -1, 1560, 1709, 1708, -1, 1560, 1708, 1558, -1, 1572, 1573, 1707, -1, 1572, 1707, 1710, -1, 1561, 1709, 1560, -1, 1571, 1572, 1710, -1, 1562, 1709, 1561, -1, 1570, 1571, 1710, -1, 1570, 1710, 1711, -1, 1563, 1712, 1709, -1, 1563, 1709, 1562, -1, 1569, 1570, 1711, -1, 1564, 1712, 1563, -1, 1568, 1711, 1713, -1, 1568, 1569, 1711, -1, 1565, 1713, 1712, -1, 1565, 1712, 1564, -1, 1567, 1568, 1713, -1, 1566, 1713, 1565, -1, 1566, 1567, 1713, -1, 1714, 1576, 1578, -1, 1714, 1715, 1576, -1, 1716, 1578, 1580, -1, 1716, 1714, 1578, -1, 1717, 1580, 1582, -1, 1717, 1716, 1580, -1, 1718, 1582, 1584, -1, 1718, 1717, 1582, -1, 1719, 1584, 1586, -1, 1719, 1718, 1584, -1, 1720, 1719, 1586, -1, 1588, 1720, 1586, -1, 1721, 1720, 1588, -1, 1590, 1721, 1588, -1, 1722, 1721, 1590, -1, 1592, 1722, 1590, -1, 1723, 1722, 1592, -1, 1594, 1723, 1592, -1, 1724, 1723, 1594, -1, 1596, 1724, 1594, -1, 1725, 1724, 1596, -1, 1598, 1725, 1596, -1, 1726, 1725, 1598, -1, 1600, 1726, 1598, -1, 1727, 1726, 1600, -1, 1602, 1727, 1600, -1, 1728, 1727, 1602, -1, 1604, 1728, 1602, -1, 1729, 1728, 1604, -1, 1606, 1729, 1604, -1, 1730, 1729, 1606, -1, 1608, 1730, 1606, -1, 1731, 1730, 1608, -1, 1610, 1731, 1608, -1, 1732, 1731, 1610, -1, 1612, 1732, 1610, -1, 1733, 1732, 1612, -1, 1614, 1733, 1612, -1, 1734, 1733, 1614, -1, 1616, 1734, 1614, -1, 1735, 1734, 1616, -1, 1618, 1735, 1616, -1, 1736, 1735, 1618, -1, 1620, 1736, 1618, -1, 1737, 1736, 1620, -1, 1622, 1737, 1620, -1, 1738, 1737, 1622, -1, 1624, 1738, 1622, -1, 1739, 1738, 1624, -1, 1626, 1739, 1624, -1, 1740, 1739, 1626, -1, 1628, 1740, 1626, -1, 1741, 1740, 1628, -1, 1630, 1741, 1628, -1, 1742, 1741, 1630, -1, 1632, 1742, 1630, -1, 1743, 1742, 1632, -1, 1634, 1743, 1632, -1, 1744, 1743, 1634, -1, 1636, 1744, 1634, -1, 1745, 1744, 1636, -1, 1638, 1745, 1636, -1, 1746, 1745, 1638, -1, 1640, 1746, 1638, -1, 1747, 1746, 1640, -1, 1642, 1747, 1640, -1, 1748, 1747, 1642, -1, 1644, 1748, 1642, -1, 1749, 1748, 1644, -1, 1646, 1749, 1644, -1, 1750, 1749, 1646, -1, 1648, 1750, 1646, -1, 1751, 1750, 1648, -1, 1650, 1751, 1648, -1, 1752, 1751, 1650, -1, 1652, 1752, 1650, -1, 1753, 1752, 1652, -1, 1654, 1753, 1652, -1, 1754, 1753, 1654, -1, 1656, 1754, 1654, -1, 1755, 1754, 1656, -1, 1658, 1755, 1656, -1, 1756, 1755, 1658, -1, 1660, 1756, 1658, -1, 1757, 1756, 1660, -1, 1662, 1757, 1660, -1, 1758, 1757, 1662, -1, 1664, 1758, 1662, -1, 1759, 1758, 1664, -1, 1666, 1759, 1664, -1, 1760, 1759, 1666, -1, 1668, 1760, 1666, -1, 1761, 1760, 1668, -1, 1670, 1761, 1668, -1, 1762, 1761, 1670, -1, 1672, 1762, 1670, -1, 1763, 1762, 1672, -1, 1674, 1763, 1672, -1, 1764, 1763, 1674, -1, 1676, 1764, 1674, -1, 1765, 1764, 1676, -1, 1678, 1765, 1676, -1, 1766, 1765, 1678, -1, 1680, 1766, 1678, -1, 1767, 1766, 1680, -1, 1682, 1767, 1680, -1, 1768, 1767, 1682, -1, 1684, 1768, 1682, -1, 1769, 1768, 1684, -1, 1686, 1769, 1684, -1, 1770, 1769, 1686, -1, 1576, 1770, 1686, -1, 1715, 1770, 1576, -1, 1771, 1772, 1707, -1, 1705, 1771, 1707, -1, 1773, 1771, 1705, -1, 1704, 1773, 1705, -1, 1774, 1773, 1704, -1, 1702, 1774, 1704, -1, 1775, 1774, 1702, -1, 1700, 1775, 1702, -1, 1776, 1775, 1700, -1, 1697, 1776, 1700, -1, 1777, 1776, 1697, -1, 1696, 1777, 1697, -1, 1778, 1777, 1696, -1, 1693, 1778, 1696, -1, 1779, 1778, 1693, -1, 1691, 1779, 1693, -1, 1780, 1779, 1691, -1, 1781, 1780, 1691, -1, 1690, 1781, 1691, -1, 1782, 1781, 1690, -1, 1692, 1782, 1690, -1, 1783, 1782, 1692, -1, 1694, 1783, 1692, -1, 1784, 1783, 1694, -1, 1695, 1784, 1694, -1, 1785, 1784, 1695, -1, 1698, 1785, 1695, -1, 1786, 1785, 1698, -1, 1699, 1786, 1698, -1, 1787, 1786, 1699, -1, 1701, 1787, 1699, -1, 1788, 1787, 1701, -1, 1703, 1788, 1701, -1, 1789, 1788, 1703, -1, 1790, 1789, 1703, -1, 1706, 1790, 1703, -1, 1791, 1790, 1706, -1, 1708, 1791, 1706, -1, 1792, 1791, 1708, -1, 1709, 1792, 1708, -1, 1793, 1792, 1709, -1, 1712, 1793, 1709, -1, 1794, 1793, 1712, -1, 1713, 1794, 1712, -1, 1795, 1794, 1713, -1, 1711, 1795, 1713, -1, 1796, 1795, 1711, -1, 1710, 1796, 1711, -1, 1797, 1796, 1710, -1, 1707, 1797, 1710, -1, 1772, 1797, 1707, -1, 1798, 1799, 1800, -1, 1801, 1798, 1800, -1, 1801, 1800, 1802, -1, 1803, 1799, 1798, -1, 1803, 1804, 1799, -1, 1805, 1801, 1802, -1, 1805, 1802, 1806, -1, 1807, 1804, 1803, -1, 1807, 1808, 1804, -1, 1809, 1805, 1806, -1, 1809, 1806, 1810, -1, 1811, 1808, 1807, -1, 1811, 1812, 1808, -1, 1813, 1809, 1810, -1, 1813, 1810, 1814, -1, 1815, 1812, 1811, -1, 1815, 1816, 1812, -1, 1817, 1813, 1814, -1, 1817, 1814, 1818, -1, 1819, 1816, 1815, -1, 1819, 1820, 1816, -1, 1821, 1817, 1818, -1, 1821, 1818, 1822, -1, 1823, 1824, 1825, -1, 1823, 1825, 1820, -1, 1823, 1820, 1819, -1, 1826, 1821, 1822, -1, 1826, 1822, 1827, -1, 1828, 1824, 1823, -1, 1828, 1829, 1824, -1, 1830, 1826, 1827, -1, 1830, 1827, 1831, -1, 1832, 1833, 1829, -1, 1832, 1829, 1828, -1, 1834, 1831, 1835, -1, 1834, 1830, 1831, -1, 1836, 1837, 1833, -1, 1836, 1833, 1832, -1, 1838, 1835, 1839, -1, 1838, 1834, 1835, -1, 1840, 1837, 1836, -1, 1840, 1841, 1837, -1, 1842, 1839, 1843, -1, 1842, 1838, 1839, -1, 1844, 1841, 1840, -1, 1844, 1845, 1841, -1, 1846, 1843, 1847, -1, 1846, 1842, 1843, -1, 1848, 1845, 1844, -1, 1848, 1849, 1845, -1, 1850, 1847, 1851, -1, 1850, 1846, 1847, -1, 1852, 1849, 1848, -1, 1853, 1850, 1851, -1, 1854, 1853, 1851, -1, 1855, 1853, 1854, -1, 1856, 1849, 1852, -1, 1856, 1852, 1857, -1, 1858, 1855, 1854, -1, 1858, 1859, 1855, -1, 1860, 1857, 1861, -1, 1860, 1856, 1857, -1, 1862, 1859, 1858, -1, 1862, 1863, 1859, -1, 1864, 1860, 1861, -1, 1864, 1861, 1865, -1, 1866, 1863, 1862, -1, 1866, 1867, 1863, -1, 1868, 1865, 1869, -1, 1868, 1864, 1865, -1, 1870, 1867, 1866, -1, 1870, 1871, 1867, -1, 1872, 1869, 1873, -1, 1872, 1868, 1869, -1, 1874, 1875, 1871, -1, 1874, 1871, 1870, -1, 1876, 1873, 1877, -1, 1876, 1872, 1873, -1, 1878, 1879, 1875, -1, 1878, 1875, 1874, -1, 1880, 1877, 1881, -1, 1880, 1876, 1877, -1, 1882, 1879, 1878, -1, 1883, 1881, 1884, -1, 1883, 1880, 1881, -1, 1885, 1886, 1879, -1, 1885, 1879, 1882, -1, 1887, 1883, 1884, -1, 1887, 1884, 1888, -1, 1889, 1890, 1886, -1, 1889, 1886, 1885, -1, 1891, 1888, 1892, -1, 1891, 1887, 1888, -1, 1893, 1894, 1890, -1, 1893, 1890, 1889, -1, 1895, 1892, 1896, -1, 1895, 1891, 1892, -1, 1897, 1898, 1894, -1, 1897, 1894, 1893, -1, 1899, 1896, 1900, -1, 1899, 1895, 1896, -1, 1901, 1902, 1898, -1, 1901, 1898, 1897, -1, 1903, 1900, 1904, -1, 1903, 1899, 1900, -1, 1905, 1906, 1902, -1, 1905, 1902, 1901, -1, 1907, 1904, 1908, -1, 1907, 1903, 1904, -1, 1909, 1907, 1908, -1, 1909, 1908, 1906, -1, 1909, 1906, 1905, -1, 1910, 1911, 1912, -1, 1910, 1912, 1913, -1, 1910, 1913, 1914, -1, 1910, 1914, 1915, -1, 1910, 1915, 1916, -1, 1910, 1916, 1917, -1, 1910, 1917, 1918, -1, 1910, 1918, 1919, -1, 1910, 1919, 1920, -1, 1910, 1920, 1921, -1, 1910, 1921, 1922, -1, 1910, 1922, 1923, -1, 1910, 1923, 1924, -1, 1910, 1924, 1925, -1, 1910, 1925, 1926, -1, 1910, 1926, 1927, -1, 1910, 1927, 1928, -1, 1910, 1928, 1929, -1, 1910, 1929, 1930, -1, 1910, 1930, 1931, -1, 1910, 1931, 1932, -1, 1910, 1932, 1933, -1, 1910, 1933, 1934, -1, 1910, 1934, 1935, -1, 1910, 1935, 1936, -1, 1910, 1936, 1937, -1, 1910, 1937, 1911, -1, 1938, 1803, 1798, -1, 1938, 1798, 1801, -1, 1938, 1801, 1805, -1, 1939, 1803, 1938, -1, 1939, 1811, 1807, -1, 1939, 1807, 1803, -1, 1940, 1805, 1809, -1, 1940, 1809, 1813, -1, 1940, 1938, 1805, -1, 1941, 1811, 1939, -1, 1941, 1819, 1815, -1, 1941, 1815, 1811, -1, 1942, 1813, 1817, -1, 1942, 1817, 1821, -1, 1942, 1940, 1813, -1, 1943, 1819, 1941, -1, 1943, 1832, 1828, -1, 1943, 1828, 1823, -1, 1943, 1823, 1819, -1, 1944, 1821, 1826, -1, 1944, 1826, 1830, -1, 1944, 1942, 1821, -1, 1945, 1840, 1836, -1, 1945, 1836, 1832, -1, 1945, 1832, 1943, -1, 1946, 1944, 1830, -1, 1946, 1830, 1834, -1, 1946, 1834, 1838, -1, 1946, 1838, 1842, -1, 1947, 1840, 1945, -1, 1947, 1848, 1844, -1, 1947, 1844, 1840, -1, 1948, 1946, 1842, -1, 1948, 1842, 1846, -1, 1850, 1948, 1846, -1, 1949, 1852, 1848, -1, 1949, 1848, 1947, -1, 1950, 1948, 1850, -1, 1950, 1850, 1853, -1, 1857, 1852, 1949, -1, 1855, 1950, 1853, -1, 1951, 1950, 1855, -1, 1861, 1949, 1952, -1, 1861, 1857, 1949, -1, 1859, 1951, 1855, -1, 1865, 1861, 1952, -1, 1863, 1951, 1859, -1, 1869, 1952, 1953, -1, 1869, 1865, 1952, -1, 1867, 1954, 1951, -1, 1867, 1951, 1863, -1, 1873, 1869, 1953, -1, 1871, 1954, 1867, -1, 1877, 1953, 1955, -1, 1877, 1873, 1953, -1, 1875, 1954, 1871, -1, 1875, 1956, 1954, -1, 1881, 1877, 1955, -1, 1879, 1956, 1875, -1, 1884, 1955, 1957, -1, 1884, 1881, 1955, -1, 1886, 1956, 1879, -1, 1886, 1958, 1956, -1, 1888, 1884, 1957, -1, 1890, 1958, 1886, -1, 1892, 1888, 1957, -1, 1894, 1958, 1890, -1, 1894, 1959, 1958, -1, 1896, 1957, 1960, -1, 1896, 1892, 1957, -1, 1898, 1959, 1894, -1, 1900, 1896, 1960, -1, 1902, 1959, 1898, -1, 1904, 1960, 1961, -1, 1904, 1900, 1960, -1, 1906, 1961, 1959, -1, 1906, 1959, 1902, -1, 1908, 1904, 1961, -1, 1908, 1961, 1906, -1, 1962, 1956, 1963, -1, 1954, 1956, 1962, -1, 1964, 1954, 1962, -1, 1951, 1954, 1964, -1, 1965, 1951, 1964, -1, 1950, 1951, 1965, -1, 1966, 1950, 1965, -1, 1948, 1950, 1966, -1, 1967, 1948, 1966, -1, 1946, 1948, 1967, -1, 1968, 1946, 1967, -1, 1944, 1946, 1968, -1, 1969, 1944, 1968, -1, 1942, 1944, 1969, -1, 1970, 1942, 1969, -1, 1940, 1942, 1970, -1, 1971, 1940, 1970, -1, 1938, 1940, 1971, -1, 1972, 1938, 1971, -1, 1939, 1938, 1972, -1, 1973, 1939, 1972, -1, 1941, 1939, 1973, -1, 1974, 1941, 1973, -1, 1943, 1941, 1974, -1, 1975, 1943, 1974, -1, 1945, 1943, 1975, -1, 1976, 1945, 1975, -1, 1947, 1945, 1976, -1, 1977, 1947, 1976, -1, 1949, 1947, 1977, -1, 1978, 1949, 1977, -1, 1952, 1949, 1978, -1, 1979, 1952, 1978, -1, 1953, 1952, 1979, -1, 1980, 1953, 1979, -1, 1955, 1953, 1980, -1, 1981, 1955, 1980, -1, 1957, 1955, 1981, -1, 1982, 1957, 1981, -1, 1960, 1957, 1982, -1, 1983, 1960, 1982, -1, 1961, 1960, 1983, -1, 1984, 1961, 1983, -1, 1959, 1961, 1984, -1, 1985, 1959, 1984, -1, 1958, 1985, 1986, -1, 1958, 1959, 1985, -1, 1956, 1986, 1963, -1, 1956, 1958, 1986, -1, 1987, 1963, 1988, -1, 1989, 1987, 1988, -1, 1990, 1989, 1988, -1, 1991, 1990, 1988, -1, 1992, 1991, 1988, -1, 1993, 1992, 1988, -1, 1994, 1993, 1988, -1, 1995, 1994, 1988, -1, 1996, 1995, 1988, -1, 1997, 1996, 1988, -1, 1998, 1997, 1988, -1, 1999, 1998, 1988, -1, 2000, 1999, 1988, -1, 2001, 2000, 1988, -1, 2002, 2001, 1988, -1, 2003, 2002, 1988, -1, 2004, 2003, 1988, -1, 2005, 2004, 1988, -1, 2006, 2005, 1988, -1, 2007, 2006, 1988, -1, 2008, 2007, 1988, -1, 2009, 2008, 1988, -1, 2010, 2009, 1988, -1, 2011, 2010, 1988, -1, 1963, 2011, 1988, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -21.000 0.000 162.611 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -21.000 0.000 -106.611 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -21.000 -134.611 28.000 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -21.000 134.611 28.000 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -155.611 0.000 28.000 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 113.611 0.000 28.000 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_ANKLE_P.wrl000066400000000000000000000524401207742442300231540ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 50.000 27.000 39.000 center 0.000 0.000 0.000 #translation 11.000 -1.450 -6.500 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.60 0.30 0.30 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -35.000 -11.050 -13.000, -35.000 -6.376 -7.897, -35.000 -5.261 -9.070, -35.000 -3.932 -9.995, -35.000 -2.445 -10.633, -35.000 -0.859 -10.959, -35.000 0.759 -10.959, -35.000 -7.232 -6.523, -35.000 -7.958 -1.789, -35.000 -8.040 -3.405, -35.000 -7.795 -5.005, -35.000 11.950 -13.000, -35.000 2.345 -10.633, -35.000 3.832 -9.995, -35.000 5.161 -9.070, -35.000 6.276 -7.897, -35.000 7.132 -6.523, -35.000 7.695 -5.005, -35.000 7.940 -3.405, -35.000 7.858 -1.789, -35.000 11.950 26.000, -11.000 11.950 26.000, -11.000 11.950 -13.000, -11.000 13.950 26.000, -11.000 13.950 -13.000, 5.719 13.950 -4.472, 5.968 13.950 -3.501, 13.000 13.950 -13.000, 5.968 13.950 -2.499, 5.719 13.950 -1.528, 13.000 13.950 26.000, 1.000 13.950 6.500, 1.000 13.950 16.250, 4.550 13.950 0.082, 5.236 13.950 -0.649, 3.703 13.950 0.619, 2.750 13.950 0.929, 1.749 13.950 0.992, 0.764 13.950 0.804, -0.143 13.950 0.377, -0.916 13.950 -0.262, -1.505 13.950 -1.073, -1.874 13.950 -2.005, -2.000 13.950 -3.000, -1.874 13.950 -3.995, -1.505 13.950 -4.927, -0.916 13.950 -5.738, -0.143 13.950 -6.377, 0.764 13.950 -6.804, 1.749 13.950 -6.992, 3.703 13.950 -6.619, 4.550 13.950 -6.082, 2.750 13.950 -6.929, 1.000 -11.050 6.500, -2.232 -11.050 3.789, -0.778 -11.050 4.502, -11.000 -11.050 26.000, 13.000 -11.050 26.000, 1.000 -11.050 16.250, -4.566 -11.050 1.570, -3.512 -11.050 2.798, -5.352 -11.050 0.155, -5.836 -11.050 -1.390, 6.897 -11.050 3.326, 8.070 -11.050 2.211, 8.995 -11.050 0.882, 5.523 -11.050 4.182, 9.633 -11.050 -0.605, 4.005 -11.050 4.745, 9.959 -11.050 -2.191, 13.000 -11.050 -13.000, 8.995 -11.050 -6.882, 9.633 -11.050 -5.395, 9.959 -11.050 -3.809, 13.000 -3.854 -0.764, 13.000 -4.042 -1.749, 13.000 -3.979 -2.750, 13.000 -3.669 -3.703, 13.000 -3.132 -4.550, 13.000 1.450 6.500, 13.000 1.450 16.250, 13.000 -2.788 0.916, 13.000 -3.427 0.143, 13.000 -1.045 1.874, 13.000 -0.050 2.000, 13.000 -1.977 1.505, 13.000 0.945 1.874, 13.000 3.327 0.143, 13.000 2.688 0.916, 13.000 1.877 1.505, 13.000 3.754 -0.764, 13.000 1.422 -5.719, 13.000 0.451 -5.968, 13.000 2.301 -5.236, 13.000 3.032 -4.550, 13.000 3.569 -3.703, 13.000 3.879 -2.750, 13.000 3.942 -1.749, 13.000 -1.522 -5.719, 13.000 -0.551 -5.968, 7.000 1.450 -13.000, 1.000 1.450 -13.000, -11.000 -11.050 -13.000, 1.000 -4.800 -13.000, 1.000 7.700 -13.000, -5.000 0.450 -13.000, 13.000 -2.401 -5.236, 8.070 -11.050 -8.211, 6.897 -11.050 -9.326, 5.523 -11.050 -10.182, 4.005 -11.050 -10.745, 2.405 -11.050 -10.990, -5.836 -11.050 -4.610, -6.000 -11.050 -3.000, -5.352 -11.050 -6.155, -4.566 -11.050 -7.570, -3.512 -11.050 -8.798, -2.232 -11.050 -9.789, -0.778 -11.050 -10.502, 0.789 -11.050 -10.908, 2.405 -11.050 4.990, 0.789 -11.050 4.908, 5.236 13.950 -5.351, 14.000 -0.050 2.000, 14.000 -1.045 1.874, 14.000 -1.977 1.505, 14.000 -2.788 0.916, 14.000 -3.427 0.143, 14.000 -3.854 -0.764, 14.000 -4.042 -1.749, 14.000 -3.979 -2.750, 14.000 -3.669 -3.703, 14.000 -3.132 -4.550, 14.000 -2.401 -5.236, 14.000 -1.522 -5.719, 14.000 -0.551 -5.968, 14.000 0.451 -5.968, 14.000 1.422 -5.719, 14.000 2.301 -5.236, 14.000 3.032 -4.550, 14.000 3.569 -3.703, 14.000 3.879 -2.750, 14.000 3.942 -1.749, 14.000 3.754 -0.764, 14.000 3.327 0.143, 14.000 2.688 0.916, 14.000 1.877 1.505, 14.000 0.945 1.874, 1.000 1.450 26.000, 7.000 1.450 26.000, 1.000 -4.800 26.000, 1.000 7.700 26.000, -5.000 0.450 26.000, -35.000 -11.050 26.000, -6.000 -12.050 -3.000, -5.836 -12.050 -4.610, -5.352 -12.050 -6.155, -4.566 -12.050 -7.570, -3.512 -12.050 -8.798, -2.232 -12.050 -9.789, -0.778 -12.050 -10.502, 0.789 -12.050 -10.908, 2.405 -12.050 -10.990, 4.005 -12.050 -10.745, 5.523 -12.050 -10.182, 6.897 -12.050 -9.326, 8.070 -12.050 -8.211, 8.995 -12.050 -6.882, 9.633 -12.050 -5.395, 9.959 -12.050 -3.809, 9.959 -12.050 -2.191, 9.633 -12.050 -0.605, 8.995 -12.050 0.882, 8.070 -12.050 2.211, 6.897 -12.050 3.326, 5.523 -12.050 4.182, 4.005 -12.050 4.745, 2.405 -12.050 4.990, 0.789 -12.050 4.908, -0.778 -12.050 4.502, -2.232 -12.050 3.789, -3.512 -12.050 2.798, -4.566 -12.050 1.570, -5.352 -12.050 0.155, -5.836 -12.050 -1.390, -1.874 14.950 -3.995, -2.000 14.950 -3.000, -1.505 14.950 -4.927, -0.916 14.950 -5.738, -0.143 14.950 -6.377, 0.764 14.950 -6.804, 1.749 14.950 -6.992, 2.750 14.950 -6.929, 3.703 14.950 -6.619, 4.550 14.950 -6.082, 5.236 14.950 -5.351, 5.719 14.950 -4.472, 5.968 14.950 -3.501, 5.968 14.950 -2.499, 5.719 14.950 -1.528, 5.236 14.950 -0.649, 4.550 14.950 0.082, 3.703 14.950 0.619, 2.750 14.950 0.929, 1.749 14.950 0.992, 0.764 14.950 0.804, -0.143 14.950 0.377, -0.916 14.950 -0.262, -1.505 14.950 -1.073, -1.874 14.950 -2.005, -35.000 7.452 -0.222, -35.000 -5.848 2.512, -35.000 -4.620 3.566, -35.000 -6.839 1.232, -35.000 -3.205 4.352, -35.000 1.560 4.836, -35.000 0.450 6.500, -35.000 3.105 4.352, -35.000 -0.050 5.000, -35.000 0.450 16.250, -35.000 -7.552 -0.222, -35.000 4.520 3.566, -35.000 5.748 2.512, -35.000 6.739 1.232, -35.000 -1.660 4.836, 14.000 -0.050 -1.984, 1.979 -12.050 -3.000, 1.984 14.950 -3.000, -36.000 -1.660 4.836, -36.000 -0.050 5.000, -36.000 -3.205 4.352, -36.000 -4.620 3.566, -36.000 -5.848 2.512, -36.000 -6.839 1.232, -36.000 -7.552 -0.222, -36.000 -7.958 -1.789, -36.000 -8.040 -3.405, -36.000 -7.795 -5.005, -36.000 -7.232 -6.523, -36.000 -6.376 -7.897, -36.000 -5.261 -9.070, -36.000 -3.932 -9.995, -36.000 -2.445 -10.633, -36.000 -0.859 -10.959, -36.000 0.759 -10.959, -36.000 2.345 -10.633, -36.000 3.832 -9.995, -36.000 5.161 -9.070, -36.000 6.276 -7.897, -36.000 7.132 -6.523, -36.000 7.695 -5.005, -36.000 7.940 -3.405, -36.000 7.858 -1.789, -36.000 7.452 -0.222, -36.000 6.739 1.232, -36.000 5.748 2.512, -36.000 4.520 3.566, -36.000 3.105 4.352, -36.000 1.560 4.836, -36.000 -0.050 -2.979 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 0, 2, 3, -1, 0, 3, 4, -1, 5, 0, 4, -1, 6, 0, 5, -1, 7, 1, 0, -1, 0, 8, 9, -1, 9, 10, 0, -1, 10, 7, 0, -1, 11, 0, 6, -1, 11, 6, 12, -1, 12, 13, 11, -1, 13, 14, 11, -1, 14, 15, 11, -1, 15, 16, 11, -1, 16, 17, 11, -1, 11, 17, 18, -1, 11, 18, 19, -1, 20, 21, 11, -1, 11, 21, 22, -1, 21, 23, 22, -1, 22, 23, 24, -1, 25, 26, 27, -1, 28, 27, 26, -1, 29, 27, 28, -1, 30, 27, 29, -1, 31, 23, 32, -1, 23, 30, 32, -1, 30, 31, 32, -1, 31, 30, 33, -1, 33, 30, 34, -1, 30, 29, 34, -1, 35, 31, 33, -1, 36, 31, 35, -1, 37, 31, 36, -1, 38, 31, 37, -1, 39, 31, 38, -1, 40, 31, 39, -1, 31, 40, 41, -1, 23, 41, 42, -1, 23, 42, 24, -1, 24, 43, 44, -1, 24, 44, 45, -1, 24, 45, 46, -1, 24, 46, 47, -1, 24, 47, 48, -1, 24, 48, 49, -1, 42, 43, 24, -1, 24, 49, 27, -1, 50, 51, 27, -1, 52, 50, 27, -1, 49, 52, 27, -1, 53, 54, 55, -1, 54, 53, 56, -1, 57, 56, 58, -1, 56, 53, 58, -1, 53, 57, 58, -1, 59, 60, 56, -1, 61, 59, 56, -1, 62, 61, 56, -1, 63, 64, 57, -1, 65, 57, 64, -1, 66, 63, 57, -1, 67, 57, 65, -1, 68, 66, 57, -1, 53, 68, 57, -1, 69, 57, 67, -1, 70, 57, 69, -1, 71, 70, 72, -1, 70, 73, 72, -1, 70, 69, 73, -1, 74, 70, 75, -1, 70, 76, 75, -1, 70, 77, 76, -1, 70, 78, 77, -1, 70, 74, 57, -1, 79, 30, 80, -1, 30, 57, 80, -1, 57, 79, 80, -1, 81, 79, 57, -1, 82, 81, 57, -1, 74, 82, 57, -1, 83, 84, 79, -1, 85, 83, 79, -1, 85, 79, 81, -1, 84, 86, 79, -1, 87, 79, 88, -1, 89, 88, 79, -1, 86, 89, 79, -1, 79, 87, 30, -1, 87, 90, 30, -1, 90, 27, 30, -1, 91, 92, 27, -1, 93, 91, 27, -1, 94, 93, 27, -1, 95, 94, 27, -1, 95, 27, 96, -1, 96, 27, 97, -1, 97, 27, 90, -1, 92, 70, 27, -1, 98, 70, 99, -1, 99, 70, 92, -1, 27, 70, 100, -1, 70, 101, 100, -1, 101, 27, 100, -1, 70, 102, 103, -1, 102, 101, 103, -1, 101, 70, 103, -1, 24, 27, 104, -1, 27, 101, 104, -1, 101, 24, 104, -1, 102, 22, 105, -1, 22, 101, 105, -1, 101, 102, 105, -1, 101, 22, 24, -1, 78, 70, 106, -1, 106, 70, 98, -1, 107, 70, 71, -1, 107, 108, 70, -1, 108, 109, 70, -1, 109, 110, 70, -1, 111, 70, 110, -1, 102, 70, 111, -1, 102, 112, 113, -1, 102, 114, 112, -1, 102, 115, 114, -1, 102, 116, 115, -1, 116, 102, 117, -1, 117, 102, 118, -1, 102, 119, 118, -1, 102, 111, 119, -1, 102, 113, 62, -1, 102, 62, 56, -1, 60, 54, 56, -1, 53, 120, 68, -1, 121, 120, 53, -1, 55, 121, 53, -1, 23, 31, 41, -1, 122, 25, 27, -1, 51, 122, 27, -1, 22, 102, 0, -1, 22, 0, 11, -1, 123, 84, 83, -1, 124, 123, 83, -1, 83, 85, 124, -1, 124, 85, 125, -1, 85, 81, 125, -1, 125, 81, 126, -1, 82, 126, 81, -1, 127, 126, 82, -1, 74, 127, 82, -1, 74, 128, 127, -1, 74, 75, 128, -1, 75, 129, 128, -1, 76, 129, 75, -1, 130, 129, 76, -1, 77, 130, 76, -1, 77, 131, 130, -1, 77, 78, 131, -1, 132, 131, 78, -1, 132, 78, 106, -1, 133, 132, 106, -1, 133, 106, 98, -1, 134, 133, 98, -1, 134, 98, 99, -1, 135, 134, 99, -1, 135, 99, 92, -1, 136, 135, 92, -1, 136, 92, 91, -1, 137, 136, 91, -1, 137, 91, 93, -1, 137, 93, 138, -1, 138, 93, 94, -1, 138, 94, 139, -1, 95, 139, 94, -1, 95, 140, 139, -1, 95, 96, 140, -1, 141, 140, 96, -1, 97, 141, 96, -1, 97, 142, 141, -1, 90, 142, 97, -1, 90, 143, 142, -1, 90, 144, 143, -1, 144, 90, 87, -1, 87, 88, 144, -1, 88, 145, 144, -1, 145, 88, 146, -1, 146, 88, 89, -1, 146, 89, 86, -1, 146, 86, 147, -1, 123, 147, 84, -1, 147, 86, 84, -1, 148, 57, 149, -1, 57, 30, 149, -1, 30, 148, 149, -1, 148, 56, 150, -1, 56, 57, 150, -1, 57, 148, 150, -1, 148, 30, 151, -1, 30, 23, 151, -1, 23, 148, 151, -1, 148, 21, 152, -1, 21, 56, 152, -1, 56, 148, 152, -1, 148, 23, 21, -1, 0, 102, 56, -1, 153, 0, 56, -1, 154, 113, 112, -1, 155, 154, 112, -1, 155, 112, 156, -1, 156, 112, 114, -1, 156, 114, 115, -1, 156, 115, 157, -1, 157, 115, 116, -1, 158, 157, 116, -1, 117, 158, 116, -1, 159, 158, 117, -1, 118, 159, 117, -1, 160, 159, 118, -1, 118, 119, 160, -1, 161, 160, 119, -1, 119, 111, 161, -1, 111, 162, 161, -1, 111, 110, 162, -1, 163, 162, 110, -1, 110, 109, 163, -1, 109, 164, 163, -1, 108, 164, 109, -1, 165, 164, 108, -1, 108, 107, 165, -1, 107, 166, 165, -1, 107, 71, 166, -1, 167, 166, 71, -1, 167, 71, 72, -1, 167, 72, 168, -1, 168, 72, 73, -1, 169, 168, 73, -1, 169, 73, 69, -1, 170, 169, 69, -1, 170, 69, 67, -1, 171, 170, 67, -1, 171, 67, 65, -1, 172, 171, 65, -1, 172, 65, 64, -1, 173, 172, 64, -1, 64, 63, 173, -1, 63, 174, 173, -1, 63, 66, 174, -1, 66, 175, 174, -1, 66, 68, 175, -1, 68, 176, 175, -1, 68, 120, 176, -1, 120, 177, 176, -1, 120, 121, 177, -1, 121, 178, 177, -1, 121, 55, 178, -1, 55, 179, 178, -1, 55, 54, 179, -1, 54, 180, 179, -1, 54, 60, 180, -1, 60, 181, 180, -1, 60, 182, 181, -1, 182, 60, 59, -1, 59, 61, 182, -1, 182, 61, 183, -1, 183, 61, 184, -1, 61, 62, 184, -1, 154, 184, 113, -1, 62, 113, 184, -1, 43, 185, 44, -1, 186, 185, 43, -1, 45, 44, 187, -1, 44, 185, 187, -1, 46, 45, 188, -1, 187, 188, 45, -1, 189, 47, 46, -1, 189, 46, 188, -1, 190, 48, 47, -1, 189, 190, 47, -1, 49, 48, 191, -1, 190, 191, 48, -1, 52, 49, 192, -1, 192, 49, 191, -1, 50, 52, 193, -1, 193, 52, 192, -1, 194, 51, 50, -1, 193, 194, 50, -1, 122, 51, 195, -1, 194, 195, 51, -1, 25, 122, 196, -1, 122, 195, 196, -1, 196, 197, 25, -1, 25, 197, 26, -1, 26, 197, 198, -1, 28, 26, 198, -1, 198, 199, 28, -1, 28, 199, 29, -1, 29, 199, 200, -1, 29, 200, 34, -1, 33, 34, 201, -1, 34, 200, 201, -1, 202, 35, 33, -1, 201, 202, 33, -1, 36, 35, 203, -1, 202, 203, 35, -1, 37, 36, 204, -1, 203, 204, 36, -1, 205, 38, 37, -1, 204, 205, 37, -1, 205, 39, 38, -1, 39, 205, 206, -1, 40, 39, 207, -1, 206, 207, 39, -1, 40, 207, 41, -1, 41, 207, 208, -1, 42, 41, 209, -1, 41, 208, 209, -1, 42, 186, 43, -1, 209, 186, 42, -1, 210, 11, 19, -1, 210, 20, 11, -1, 211, 153, 212, -1, 153, 211, 213, -1, 212, 153, 214, -1, 215, 216, 217, -1, 218, 216, 215, -1, 216, 20, 217, -1, 153, 20, 219, -1, 20, 216, 219, -1, 216, 153, 219, -1, 214, 153, 216, -1, 153, 213, 220, -1, 153, 220, 8, -1, 217, 20, 221, -1, 221, 20, 222, -1, 222, 20, 223, -1, 210, 223, 20, -1, 153, 8, 0, -1, 224, 216, 218, -1, 216, 224, 214, -1, 124, 225, 123, -1, 125, 225, 124, -1, 126, 225, 125, -1, 127, 225, 126, -1, 127, 128, 225, -1, 128, 129, 225, -1, 129, 130, 225, -1, 130, 131, 225, -1, 131, 132, 225, -1, 132, 133, 225, -1, 133, 134, 225, -1, 134, 135, 225, -1, 135, 136, 225, -1, 136, 137, 225, -1, 137, 138, 225, -1, 225, 138, 139, -1, 225, 139, 140, -1, 225, 140, 141, -1, 225, 141, 142, -1, 225, 142, 143, -1, 225, 143, 144, -1, 225, 144, 145, -1, 146, 225, 145, -1, 147, 225, 146, -1, 123, 225, 147, -1, 56, 21, 153, -1, 21, 20, 153, -1, 154, 155, 226, -1, 155, 156, 226, -1, 156, 157, 226, -1, 157, 158, 226, -1, 158, 159, 226, -1, 159, 160, 226, -1, 160, 161, 226, -1, 161, 162, 226, -1, 162, 163, 226, -1, 163, 164, 226, -1, 164, 165, 226, -1, 226, 165, 166, -1, 226, 166, 167, -1, 226, 167, 168, -1, 226, 168, 169, -1, 226, 169, 170, -1, 226, 170, 171, -1, 226, 171, 172, -1, 226, 172, 173, -1, 174, 226, 173, -1, 175, 226, 174, -1, 176, 226, 175, -1, 177, 226, 176, -1, 178, 226, 177, -1, 179, 226, 178, -1, 180, 226, 179, -1, 181, 226, 180, -1, 182, 226, 181, -1, 182, 183, 226, -1, 183, 184, 226, -1, 184, 154, 226, -1, 185, 186, 227, -1, 187, 185, 227, -1, 188, 187, 227, -1, 188, 227, 189, -1, 189, 227, 190, -1, 190, 227, 191, -1, 191, 227, 192, -1, 192, 227, 193, -1, 193, 227, 194, -1, 227, 195, 194, -1, 227, 196, 195, -1, 227, 197, 196, -1, 227, 198, 197, -1, 227, 199, 198, -1, 227, 200, 199, -1, 227, 201, 200, -1, 202, 201, 227, -1, 203, 202, 227, -1, 204, 203, 227, -1, 205, 204, 227, -1, 206, 205, 227, -1, 207, 206, 227, -1, 208, 207, 227, -1, 209, 208, 227, -1, 186, 209, 227, -1, 224, 218, 228, -1, 218, 229, 228, -1, 224, 228, 214, -1, 214, 228, 230, -1, 212, 214, 231, -1, 214, 230, 231, -1, 212, 232, 211, -1, 212, 231, 232, -1, 213, 211, 233, -1, 232, 233, 211, -1, 234, 220, 213, -1, 233, 234, 213, -1, 8, 220, 235, -1, 235, 220, 234, -1, 236, 9, 8, -1, 235, 236, 8, -1, 236, 237, 9, -1, 10, 9, 237, -1, 237, 238, 10, -1, 238, 7, 10, -1, 238, 239, 7, -1, 239, 1, 7, -1, 240, 2, 1, -1, 239, 240, 1, -1, 2, 241, 3, -1, 2, 240, 241, -1, 4, 3, 242, -1, 3, 241, 242, -1, 4, 243, 5, -1, 4, 242, 243, -1, 6, 5, 244, -1, 5, 243, 244, -1, 12, 6, 245, -1, 6, 244, 245, -1, 12, 246, 13, -1, 12, 245, 246, -1, 13, 247, 14, -1, 13, 246, 247, -1, 15, 14, 248, -1, 248, 14, 247, -1, 16, 15, 249, -1, 248, 249, 15, -1, 17, 16, 250, -1, 249, 250, 16, -1, 251, 18, 17, -1, 250, 251, 17, -1, 19, 18, 252, -1, 252, 18, 251, -1, 253, 210, 19, -1, 252, 253, 19, -1, 223, 210, 254, -1, 254, 210, 253, -1, 222, 223, 255, -1, 254, 255, 223, -1, 222, 255, 221, -1, 221, 255, 256, -1, 217, 221, 257, -1, 221, 256, 257, -1, 217, 257, 215, -1, 215, 257, 258, -1, 218, 215, 229, -1, 258, 229, 215, -1, 228, 229, 259, -1, 230, 228, 259, -1, 231, 230, 259, -1, 232, 231, 259, -1, 233, 232, 259, -1, 234, 233, 259, -1, 235, 234, 259, -1, 236, 235, 259, -1, 237, 236, 259, -1, 238, 237, 259, -1, 239, 238, 259, -1, 239, 259, 240, -1, 240, 259, 241, -1, 241, 259, 242, -1, 242, 259, 243, -1, 243, 259, 244, -1, 244, 259, 245, -1, 245, 259, 246, -1, 246, 259, 247, -1, 259, 248, 247, -1, 259, 249, 248, -1, 259, 250, 249, -1, 259, 251, 250, -1, 259, 252, 251, -1, 259, 253, 252, -1, 259, 254, 253, -1, 259, 255, 254, -1, 259, 256, 255, -1, 257, 256, 259, -1, 258, 257, 259, -1, 229, 258, 259, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -11.000 1.450 75.420 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -11.000 1.450 -62.420 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -11.000 -67.470 6.500 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -11.000 70.370 6.500 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -79.920 1.450 6.500 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 57.920 1.450 6.500 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_ANKLE_R.wrl000066400000000000000000000630231207742442300231550ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 85.700 57.080 29.202 center 0.000 0.000 0.000 #translation 12.370 -13.610 6.649 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -55.220 36.150 -19.250, -55.220 36.150 -21.250, -55.220 -8.930 -19.250, -55.220 -8.930 -21.250, 17.190 12.073 -19.250, 17.190 -12.271 -19.250, 30.480 -8.930 -19.250, -50.773 41.946 -19.250, -53.463 40.393 -19.250, -49.220 42.150 -19.250, -41.110 12.073 -19.250, -38.810 12.073 -19.250, 30.480 36.150 -19.250, -54.416 -11.930 -19.250, -53.463 -13.173 -19.250, -52.220 -14.126 -19.250, 24.480 42.150 -19.250, 14.890 12.073 -19.250, -49.220 -14.930 -19.250, -50.773 -14.726 -19.250, 27.480 41.346 -19.250, 30.276 37.703 -19.250, 26.033 41.946 -19.250, -55.016 -10.483 -19.250, 29.676 39.150 -19.250, 28.723 40.393 -19.250, -12.370 13.610 -19.250, 14.890 -12.930 -19.250, 14.890 -12.897 -19.250, 14.890 -12.271 -19.250, -38.810 -12.897 -19.250, -38.810 -12.930 -19.250, -38.810 -12.271 -19.250, 17.190 -12.897 -19.250, -41.110 -12.897 -19.250, -41.110 -12.930 -19.250, 24.480 -14.930 -19.250, 17.190 -12.930 -19.250, -41.110 -12.271 -19.250, 27.480 -14.126 -19.250, 26.033 -14.726 -19.250, 29.676 -11.930 -19.250, 28.723 -13.173 -19.250, -54.416 39.150 -19.250, -55.016 37.703 -19.250, 30.276 -10.483 -19.250, -52.220 41.346 -19.250, -55.016 -10.483 -21.250, -54.416 -11.930 -21.250, -53.463 -13.173 -21.250, -52.220 -14.126 -21.250, -50.773 -14.726 -21.250, -49.220 -14.930 -21.250, -55.016 37.703 -21.250, -54.416 39.150 -21.250, -53.463 40.393 -21.250, -52.220 41.346 -21.250, -50.773 41.946 -21.250, -49.220 42.150 -21.250, -38.810 -12.930 -21.250, 14.890 -12.930 -21.250, 24.480 -14.930 -21.250, 26.033 -14.726 -21.250, 27.480 -14.126 -21.250, 28.723 -13.173 -21.250, 29.676 -11.930 -21.250, 30.276 -10.483 -21.250, 30.480 -8.930 -21.250, 17.190 -12.930 -21.250, -41.110 -12.930 -21.250, -41.110 12.930 -21.250, 17.190 12.930 -21.250, 30.480 36.150 -21.250, 14.890 12.930 -21.250, 24.480 42.150 -21.250, 27.480 41.346 -21.250, 30.276 37.703 -21.250, 26.033 41.946 -21.250, 29.676 39.150 -21.250, -38.810 12.930 -21.250, 28.723 40.393 -21.250, -12.370 13.610 -21.250, 14.890 -12.930 -19.243, 17.190 -12.930 -19.243, 14.890 -7.991 0.381, 17.190 -7.991 0.381, 17.190 6.426 -4.765, 17.190 5.151 -6.121, 17.190 3.597 -7.146, 17.190 7.353 -3.151, 17.190 7.882 -1.366, 17.190 7.985 0.492, 17.190 -5.114 -6.152, 17.190 -3.569 -7.160, 17.190 -6.388 -4.816, 17.190 -7.322 -3.224, 17.190 -7.866 -1.461, 17.190 -0.099 -9.379, 17.190 -1.833 -7.787, 17.190 0.000 -8.000, 17.190 1.849 -7.783, 14.890 7.985 0.492, 14.890 5.151 -6.121, 14.890 6.426 -4.765, 14.890 3.597 -7.146, 14.890 7.353 -3.151, 14.890 7.882 -1.366, 14.890 -3.569 -7.160, 14.890 -5.114 -6.152, 14.890 -6.388 -4.816, 14.890 -7.322 -3.224, 14.890 -7.866 -1.461, 14.890 -0.099 -9.379, 14.890 -1.833 -7.787, 14.890 0.000 -8.000, 14.890 1.849 -7.783, -41.110 -12.930 -19.243, -38.810 -12.930 -19.243, -41.110 -7.991 0.381, -38.810 -7.991 0.381, -38.810 -7.866 -1.461, -38.810 -7.322 -3.224, -38.810 -0.099 -9.379, -38.810 -1.833 -7.787, -38.810 -3.569 -7.160, -38.810 -0.000 -8.000, -38.810 1.849 -7.783, -38.810 3.597 -7.146, -38.810 6.426 -4.765, -38.810 5.151 -6.121, -38.810 7.353 -3.151, -38.810 7.882 -1.366, -38.810 7.985 0.492, -38.810 -5.114 -6.152, -38.810 -6.388 -4.816, -41.110 5.151 -6.121, -41.110 6.426 -4.765, -41.110 3.597 -7.146, -41.110 7.353 -3.151, -41.110 7.882 -1.366, -41.110 7.985 0.492, -41.110 -3.569 -7.160, -41.110 -5.114 -6.152, -41.110 -6.388 -4.816, -41.110 -7.322 -3.224, -41.110 -7.866 -1.461, -41.110 -0.099 -9.379, -41.110 -1.833 -7.787, -41.110 -0.000 -8.000, -41.110 1.849 -7.783, 17.190 -6.966 3.934, 14.890 -5.868 5.438, 14.890 -6.966 3.934, 17.190 -5.868 5.438, 14.890 7.655 2.325, 17.190 7.985 0.492, 17.190 -7.687 2.218, 14.890 -7.687 2.218, 17.190 7.655 2.325, 14.890 6.910 4.031, 17.190 -7.991 0.381, 17.190 6.910 4.031, 17.190 5.791 5.519, 14.890 5.791 5.519, 17.190 4.358 6.708, 14.890 4.358 6.708, 17.190 2.690 7.534, 14.890 2.690 7.534, 17.190 0.875 7.952, 14.890 0.875 7.952, 17.190 -0.986 7.939, 14.890 -0.986 7.939, 17.190 -2.795 7.496, 14.890 -2.795 7.496, 17.190 -4.452 6.647, 14.890 -4.452 6.647, 17.190 -3.864 1.035, 17.190 -3.464 2.000, 17.190 -7.638 2.000, 17.190 7.673 2.000, 17.190 3.464 2.000, 17.190 3.864 1.035, 17.190 -0.000 -4.000, 17.190 -1.035 -3.864, 17.190 -4.000 0.000, 17.190 -3.864 -1.035, 17.190 4.000 0.000, 17.190 -3.464 -2.000, 17.190 3.864 -1.035, 17.190 -2.828 -2.828, 17.190 1.035 -3.864, 17.190 -2.000 -3.464, 17.190 3.464 -2.000, 17.190 2.000 -3.464, 17.190 2.828 -2.828, 14.890 -7.638 2.000, 14.890 -3.464 2.000, 14.890 -3.864 1.035, 14.890 3.864 1.035, 14.890 3.464 2.000, 14.890 7.673 2.000, 14.890 -1.035 -3.864, 14.890 -0.000 -4.000, 14.890 -4.000 0.000, 14.890 -3.864 -1.035, 14.890 4.000 0.000, 14.890 -3.464 -2.000, 14.890 3.864 -1.035, 14.890 -2.828 -2.828, 14.890 1.035 -3.864, 14.890 -2.000 -3.464, 14.890 3.464 -2.000, 14.890 2.000 -3.464, 14.890 2.828 -2.828, -38.810 -6.966 3.934, -41.110 -5.868 5.438, -41.110 -6.966 3.934, -38.810 -5.868 5.438, -41.110 7.655 2.325, -38.810 -7.687 2.218, -41.110 -7.687 2.218, -38.810 7.655 2.325, -41.110 6.910 4.031, -38.810 6.910 4.031, -38.810 5.791 5.519, -41.110 5.791 5.519, -38.810 4.358 6.708, -41.110 4.358 6.708, -38.810 2.690 7.534, -41.110 2.690 7.534, -38.810 0.875 7.952, -41.110 0.875 7.952, -38.810 -0.986 7.939, -41.110 -0.986 7.939, -38.810 -2.795 7.496, -41.110 -2.795 7.496, -38.810 -4.452 6.647, -41.110 -4.452 6.647, -38.810 -3.864 1.035, -38.810 -3.464 2.000, -38.810 -7.638 2.000, -38.810 7.673 2.000, -38.810 3.464 2.000, -38.810 3.864 1.035, -38.810 -4.000 0.000, -38.810 -0.000 -4.000, -38.810 -1.035 -3.864, -38.810 -3.864 -1.035, -38.810 4.000 -0.000, -38.810 -3.464 -2.000, -38.810 3.864 -1.035, -38.810 -2.828 -2.828, -38.810 1.035 -3.864, -38.810 -2.000 -3.464, -38.810 3.464 -2.000, -38.810 2.000 -3.464, -38.810 2.828 -2.828, -41.110 -7.638 2.000, -41.110 -3.464 2.000, -41.110 -3.864 1.035, -41.110 3.864 1.035, -41.110 3.464 2.000, -41.110 7.673 2.000, -41.110 -4.000 0.000, -41.110 -1.035 -3.864, -41.110 -0.000 -4.000, -41.110 -3.864 -1.035, -41.110 4.000 -0.000, -41.110 -3.464 -2.000, -41.110 3.864 -1.035, -41.110 -2.828 -2.828, -41.110 1.035 -3.864, -41.110 -2.000 -3.464, -41.110 3.464 -2.000, -41.110 2.000 -3.464, -41.110 2.828 -2.828, 14.890 1.035 3.864, 14.890 2.000 3.464, 14.890 2.828 2.828, 14.890 -0.000 4.000, 14.890 -0.003 4.166, 14.890 -1.035 3.864, 14.890 -2.828 2.828, 14.890 -2.000 3.464, 17.190 2.000 3.464, 17.190 4.358 6.708, 17.190 2.690 7.534, 17.190 1.035 3.864, 17.190 2.828 2.828, 17.190 5.791 5.519, 17.190 6.910 4.031, 17.190 -6.966 3.934, 17.190 -7.687 2.218, 17.190 7.655 2.325, 17.190 -0.003 4.166, 17.190 -0.000 4.000, 17.190 -1.035 3.864, 17.190 0.875 7.952, 17.190 -0.986 7.939, 17.190 -2.828 2.828, 17.190 -4.452 6.647, 17.190 -5.868 5.438, 17.190 -2.000 3.464, 17.190 -2.795 7.496, -41.110 1.035 3.864, -41.110 2.000 3.464, -41.110 2.828 2.828, -41.110 0.000 4.000, -41.110 -0.003 4.166, -41.110 -1.035 3.864, -41.110 -2.828 2.828, -41.110 -2.000 3.464, -38.810 2.000 3.464, -38.810 1.035 3.864, -38.810 2.828 2.828, -38.810 -0.003 4.166, -38.810 0.000 4.000, -38.810 -1.035 3.864, -38.810 -2.828 2.828, -38.810 -2.000 3.464 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 1, 3, 2, -1, 4, 5, 6, -1, 7, 8, 0, -1, 9, 10, 11, -1, 9, 0, 10, -1, 12, 4, 6, -1, 9, 7, 0, -1, 13, 14, 15, -1, 16, 17, 4, -1, 16, 4, 12, -1, 18, 15, 19, -1, 20, 16, 12, -1, 20, 12, 21, -1, 20, 22, 16, -1, 2, 23, 13, -1, 24, 20, 21, -1, 2, 13, 15, -1, 2, 15, 18, -1, 25, 20, 24, -1, 26, 27, 28, -1, 26, 29, 17, -1, 26, 30, 31, -1, 26, 11, 32, -1, 26, 17, 16, -1, 33, 29, 28, -1, 26, 16, 9, -1, 26, 9, 11, -1, 5, 29, 33, -1, 26, 32, 30, -1, 34, 18, 35, -1, 26, 31, 27, -1, 26, 28, 29, -1, 36, 37, 27, -1, 38, 2, 18, -1, 36, 5, 33, -1, 36, 33, 37, -1, 38, 18, 34, -1, 36, 31, 18, -1, 36, 27, 31, -1, 31, 35, 18, -1, 39, 36, 40, -1, 32, 34, 30, -1, 32, 38, 34, -1, 41, 39, 42, -1, 10, 2, 38, -1, 0, 2, 10, -1, 6, 39, 41, -1, 6, 36, 39, -1, 43, 44, 0, -1, 6, 41, 45, -1, 6, 5, 36, -1, 8, 43, 0, -1, 7, 46, 8, -1, 23, 3, 47, -1, 23, 2, 3, -1, 13, 47, 48, -1, 13, 23, 47, -1, 14, 48, 49, -1, 14, 13, 48, -1, 15, 49, 50, -1, 15, 14, 49, -1, 19, 50, 51, -1, 19, 15, 50, -1, 18, 51, 52, -1, 18, 19, 51, -1, 53, 1, 44, -1, 1, 0, 44, -1, 54, 53, 43, -1, 53, 44, 43, -1, 55, 54, 8, -1, 54, 43, 8, -1, 56, 55, 46, -1, 55, 8, 46, -1, 57, 56, 7, -1, 56, 46, 7, -1, 58, 57, 9, -1, 57, 7, 9, -1, 59, 60, 61, -1, 62, 61, 63, -1, 50, 49, 48, -1, 64, 63, 65, -1, 51, 50, 52, -1, 48, 47, 3, -1, 50, 48, 3, -1, 66, 65, 67, -1, 52, 50, 3, -1, 61, 68, 67, -1, 63, 61, 67, -1, 52, 3, 69, -1, 65, 63, 67, -1, 52, 69, 59, -1, 69, 3, 70, -1, 67, 68, 71, -1, 70, 3, 1, -1, 67, 71, 72, -1, 71, 73, 74, -1, 72, 71, 74, -1, 1, 53, 54, -1, 1, 54, 55, -1, 72, 74, 75, -1, 76, 72, 75, -1, 74, 77, 75, -1, 55, 56, 57, -1, 76, 75, 78, -1, 1, 55, 57, -1, 79, 70, 58, -1, 70, 1, 58, -1, 78, 75, 80, -1, 73, 60, 81, -1, 1, 57, 58, -1, 59, 79, 81, -1, 58, 74, 81, -1, 60, 59, 81, -1, 79, 58, 81, -1, 74, 73, 81, -1, 60, 68, 61, -1, 52, 59, 61, -1, 82, 27, 37, -1, 82, 37, 83, -1, 83, 37, 33, -1, 83, 28, 82, -1, 33, 28, 83, -1, 28, 27, 82, -1, 84, 29, 85, -1, 29, 5, 85, -1, 86, 87, 4, -1, 88, 4, 87, -1, 89, 86, 4, -1, 90, 89, 4, -1, 91, 90, 4, -1, 92, 5, 93, -1, 94, 5, 92, -1, 95, 5, 94, -1, 85, 5, 96, -1, 96, 5, 95, -1, 97, 5, 4, -1, 97, 98, 93, -1, 97, 99, 98, -1, 97, 100, 99, -1, 97, 88, 100, -1, 97, 4, 88, -1, 97, 93, 5, -1, 91, 17, 101, -1, 91, 4, 17, -1, 17, 102, 103, -1, 102, 17, 104, -1, 17, 103, 105, -1, 17, 105, 106, -1, 17, 106, 101, -1, 107, 29, 108, -1, 108, 29, 109, -1, 109, 29, 110, -1, 111, 29, 84, -1, 110, 29, 111, -1, 17, 29, 112, -1, 107, 113, 112, -1, 113, 114, 112, -1, 114, 115, 112, -1, 115, 104, 112, -1, 104, 17, 112, -1, 29, 107, 112, -1, 116, 35, 117, -1, 35, 31, 117, -1, 117, 31, 30, -1, 34, 35, 116, -1, 117, 34, 116, -1, 30, 34, 117, -1, 118, 38, 119, -1, 38, 32, 119, -1, 119, 32, 120, -1, 120, 32, 121, -1, 122, 32, 11, -1, 122, 123, 124, -1, 122, 125, 123, -1, 122, 126, 125, -1, 122, 127, 126, -1, 122, 11, 127, -1, 122, 124, 32, -1, 128, 129, 11, -1, 127, 11, 129, -1, 130, 128, 11, -1, 131, 130, 11, -1, 132, 131, 11, -1, 133, 32, 124, -1, 134, 32, 133, -1, 121, 32, 134, -1, 10, 135, 136, -1, 135, 10, 137, -1, 10, 136, 138, -1, 10, 138, 139, -1, 10, 139, 140, -1, 141, 38, 142, -1, 142, 38, 143, -1, 143, 38, 144, -1, 145, 38, 118, -1, 144, 38, 145, -1, 10, 38, 146, -1, 141, 147, 146, -1, 147, 148, 146, -1, 148, 149, 146, -1, 149, 137, 146, -1, 137, 10, 146, -1, 38, 141, 146, -1, 132, 10, 140, -1, 132, 11, 10, -1, 52, 61, 36, -1, 18, 52, 36, -1, 16, 74, 58, -1, 16, 58, 9, -1, 66, 67, 45, -1, 67, 6, 45, -1, 65, 66, 41, -1, 66, 45, 41, -1, 64, 65, 42, -1, 65, 41, 42, -1, 63, 64, 39, -1, 64, 42, 39, -1, 62, 63, 40, -1, 63, 39, 40, -1, 61, 62, 36, -1, 62, 40, 36, -1, 21, 72, 76, -1, 21, 12, 72, -1, 24, 76, 78, -1, 24, 21, 76, -1, 25, 78, 80, -1, 25, 24, 78, -1, 20, 80, 75, -1, 20, 25, 80, -1, 22, 75, 77, -1, 22, 20, 75, -1, 16, 77, 74, -1, 16, 22, 77, -1, 6, 72, 12, -1, 6, 67, 72, -1, 68, 60, 71, -1, 60, 73, 71, -1, 59, 69, 79, -1, 69, 70, 79, -1, 150, 151, 152, -1, 150, 153, 151, -1, 154, 155, 101, -1, 156, 152, 157, -1, 158, 155, 154, -1, 156, 150, 152, -1, 159, 158, 154, -1, 160, 157, 84, -1, 161, 158, 159, -1, 160, 156, 157, -1, 162, 159, 163, -1, 162, 161, 159, -1, 164, 163, 165, -1, 164, 162, 163, -1, 166, 165, 167, -1, 166, 164, 165, -1, 168, 167, 169, -1, 168, 166, 167, -1, 170, 169, 171, -1, 170, 168, 169, -1, 172, 171, 173, -1, 172, 170, 171, -1, 174, 173, 175, -1, 174, 172, 173, -1, 153, 175, 151, -1, 153, 174, 175, -1, 176, 177, 178, -1, 179, 180, 181, -1, 98, 182, 183, -1, 184, 85, 96, -1, 184, 178, 85, -1, 98, 183, 93, -1, 184, 176, 178, -1, 185, 96, 95, -1, 185, 184, 96, -1, 91, 181, 186, -1, 91, 179, 181, -1, 187, 95, 94, -1, 99, 182, 98, -1, 187, 185, 95, -1, 90, 186, 188, -1, 90, 91, 186, -1, 189, 187, 94, -1, 100, 190, 182, -1, 92, 189, 94, -1, 100, 182, 99, -1, 191, 189, 92, -1, 89, 188, 192, -1, 89, 90, 188, -1, 88, 193, 190, -1, 88, 190, 100, -1, 93, 183, 191, -1, 86, 192, 194, -1, 86, 89, 192, -1, 93, 191, 92, -1, 87, 86, 194, -1, 87, 194, 193, -1, 87, 193, 88, -1, 195, 196, 197, -1, 198, 199, 200, -1, 201, 202, 113, -1, 111, 84, 203, -1, 84, 195, 203, -1, 107, 201, 113, -1, 195, 197, 203, -1, 110, 111, 204, -1, 111, 203, 204, -1, 205, 198, 101, -1, 198, 200, 101, -1, 109, 110, 206, -1, 113, 202, 114, -1, 110, 204, 206, -1, 207, 205, 106, -1, 205, 101, 106, -1, 109, 206, 208, -1, 202, 209, 115, -1, 109, 208, 108, -1, 114, 202, 115, -1, 108, 208, 210, -1, 211, 207, 105, -1, 207, 106, 105, -1, 209, 212, 104, -1, 115, 209, 104, -1, 210, 201, 107, -1, 213, 211, 103, -1, 211, 105, 103, -1, 108, 210, 107, -1, 213, 103, 102, -1, 212, 213, 102, -1, 104, 212, 102, -1, 214, 215, 216, -1, 214, 217, 215, -1, 218, 132, 140, -1, 219, 216, 220, -1, 221, 132, 218, -1, 219, 214, 216, -1, 222, 221, 218, -1, 119, 220, 118, -1, 223, 221, 222, -1, 119, 219, 220, -1, 224, 222, 225, -1, 224, 223, 222, -1, 226, 225, 227, -1, 226, 224, 225, -1, 228, 227, 229, -1, 228, 226, 227, -1, 230, 229, 231, -1, 230, 228, 229, -1, 232, 231, 233, -1, 232, 230, 231, -1, 234, 233, 235, -1, 234, 232, 233, -1, 236, 235, 237, -1, 236, 234, 235, -1, 217, 237, 215, -1, 217, 236, 237, -1, 238, 239, 240, -1, 241, 242, 243, -1, 244, 119, 120, -1, 123, 245, 246, -1, 244, 240, 119, -1, 244, 238, 240, -1, 123, 246, 124, -1, 247, 120, 121, -1, 247, 244, 120, -1, 132, 243, 248, -1, 132, 241, 243, -1, 249, 121, 134, -1, 125, 245, 123, -1, 249, 247, 121, -1, 131, 248, 250, -1, 131, 132, 248, -1, 251, 249, 134, -1, 126, 252, 245, -1, 133, 251, 134, -1, 126, 245, 125, -1, 253, 251, 133, -1, 130, 250, 254, -1, 130, 131, 250, -1, 127, 255, 252, -1, 127, 252, 126, -1, 128, 254, 256, -1, 124, 246, 253, -1, 128, 130, 254, -1, 124, 253, 133, -1, 129, 128, 256, -1, 129, 256, 255, -1, 129, 255, 127, -1, 257, 258, 259, -1, 260, 261, 262, -1, 145, 118, 263, -1, 264, 265, 147, -1, 118, 257, 263, -1, 257, 259, 263, -1, 141, 264, 147, -1, 144, 145, 266, -1, 145, 263, 266, -1, 267, 260, 140, -1, 260, 262, 140, -1, 143, 144, 268, -1, 147, 265, 148, -1, 144, 266, 268, -1, 269, 267, 139, -1, 267, 140, 139, -1, 143, 268, 270, -1, 265, 271, 149, -1, 143, 270, 142, -1, 148, 265, 149, -1, 142, 270, 272, -1, 273, 269, 138, -1, 269, 139, 138, -1, 271, 274, 137, -1, 149, 271, 137, -1, 275, 273, 136, -1, 273, 138, 136, -1, 272, 264, 141, -1, 275, 136, 135, -1, 274, 275, 135, -1, 142, 272, 141, -1, 137, 274, 135, -1, 276, 167, 277, -1, 277, 165, 278, -1, 278, 165, 163, -1, 278, 163, 199, -1, 199, 163, 159, -1, 157, 152, 195, -1, 199, 159, 154, -1, 199, 154, 200, -1, 157, 195, 84, -1, 200, 154, 101, -1, 276, 279, 280, -1, 279, 281, 280, -1, 171, 169, 280, -1, 169, 276, 280, -1, 281, 171, 280, -1, 151, 175, 282, -1, 175, 173, 283, -1, 282, 175, 283, -1, 152, 151, 196, -1, 195, 152, 196, -1, 151, 282, 196, -1, 173, 171, 281, -1, 283, 173, 281, -1, 169, 167, 276, -1, 167, 165, 277, -1, 284, 285, 286, -1, 284, 286, 287, -1, 288, 285, 284, -1, 289, 285, 288, -1, 180, 289, 288, -1, 290, 289, 180, -1, 178, 291, 292, -1, 293, 290, 180, -1, 179, 293, 180, -1, 85, 178, 292, -1, 91, 293, 179, -1, 294, 295, 287, -1, 294, 296, 295, -1, 294, 297, 298, -1, 294, 287, 297, -1, 294, 298, 296, -1, 299, 300, 301, -1, 302, 303, 300, -1, 302, 300, 299, -1, 177, 301, 291, -1, 177, 291, 178, -1, 177, 299, 301, -1, 296, 298, 303, -1, 296, 303, 302, -1, 287, 286, 297, -1, 208, 206, 189, -1, 277, 278, 284, -1, 206, 187, 189, -1, 278, 288, 284, -1, 202, 182, 209, -1, 210, 208, 191, -1, 208, 189, 191, -1, 276, 277, 287, -1, 277, 284, 287, -1, 209, 182, 190, -1, 201, 210, 183, -1, 210, 191, 183, -1, 279, 276, 295, -1, 212, 209, 193, -1, 276, 287, 295, -1, 209, 190, 193, -1, 202, 201, 182, -1, 201, 183, 182, -1, 281, 279, 296, -1, 213, 212, 194, -1, 279, 295, 296, -1, 212, 193, 194, -1, 281, 296, 302, -1, 283, 281, 302, -1, 211, 213, 192, -1, 213, 194, 192, -1, 283, 302, 299, -1, 282, 283, 299, -1, 207, 211, 188, -1, 211, 192, 188, -1, 282, 299, 177, -1, 196, 282, 177, -1, 207, 188, 205, -1, 197, 196, 176, -1, 205, 188, 186, -1, 196, 177, 176, -1, 198, 205, 181, -1, 203, 197, 184, -1, 197, 176, 184, -1, 205, 186, 181, -1, 199, 198, 180, -1, 204, 203, 185, -1, 203, 184, 185, -1, 198, 181, 180, -1, 278, 199, 288, -1, 206, 204, 187, -1, 204, 185, 187, -1, 199, 180, 288, -1, 304, 229, 305, -1, 305, 227, 306, -1, 306, 227, 225, -1, 306, 225, 261, -1, 261, 225, 222, -1, 261, 222, 218, -1, 220, 216, 257, -1, 261, 218, 262, -1, 262, 218, 140, -1, 304, 307, 308, -1, 307, 309, 308, -1, 233, 231, 308, -1, 220, 257, 118, -1, 309, 233, 308, -1, 231, 304, 308, -1, 215, 237, 310, -1, 216, 215, 258, -1, 257, 216, 258, -1, 215, 310, 258, -1, 237, 235, 311, -1, 310, 237, 311, -1, 235, 233, 309, -1, 311, 235, 309, -1, 231, 229, 304, -1, 229, 227, 305, -1, 312, 228, 313, -1, 314, 226, 312, -1, 224, 226, 314, -1, 242, 224, 314, -1, 223, 224, 242, -1, 240, 214, 219, -1, 221, 223, 242, -1, 241, 221, 242, -1, 132, 221, 241, -1, 119, 240, 219, -1, 315, 316, 313, -1, 315, 317, 316, -1, 315, 230, 232, -1, 315, 232, 317, -1, 315, 313, 230, -1, 318, 236, 217, -1, 319, 234, 236, -1, 319, 236, 318, -1, 239, 217, 214, -1, 239, 318, 217, -1, 239, 214, 240, -1, 317, 232, 234, -1, 317, 234, 319, -1, 313, 228, 230, -1, 312, 226, 228, -1, 272, 270, 253, -1, 306, 314, 312, -1, 270, 251, 253, -1, 305, 312, 304, -1, 265, 245, 271, -1, 264, 272, 246, -1, 272, 253, 246, -1, 304, 312, 313, -1, 304, 313, 307, -1, 271, 245, 252, -1, 265, 264, 245, -1, 264, 246, 245, -1, 307, 313, 316, -1, 271, 252, 274, -1, 307, 316, 309, -1, 274, 252, 255, -1, 309, 316, 317, -1, 309, 317, 311, -1, 274, 255, 275, -1, 275, 255, 256, -1, 311, 317, 319, -1, 311, 319, 310, -1, 273, 275, 254, -1, 310, 319, 318, -1, 275, 256, 254, -1, 310, 318, 258, -1, 269, 273, 250, -1, 258, 318, 239, -1, 273, 254, 250, -1, 259, 258, 238, -1, 258, 239, 238, -1, 269, 250, 267, -1, 267, 250, 248, -1, 263, 259, 244, -1, 259, 238, 244, -1, 260, 267, 243, -1, 267, 248, 243, -1, 266, 263, 247, -1, 263, 244, 247, -1, 261, 260, 242, -1, 260, 243, 242, -1, 268, 266, 249, -1, 266, 247, 249, -1, 306, 261, 314, -1, 261, 242, 314, -1, 270, 268, 251, -1, 268, 249, 251, -1, 305, 306, 312, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -12.370 13.610 100.381 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -12.370 13.610 -113.679 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -12.370 -93.420 -6.649 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -12.370 120.640 -6.649 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -119.400 13.610 -6.649 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 94.660 13.610 -6.649 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_ELBOW_P.wrl000066400000000000000000001736021207742442300231760ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 43.000 28.000 79.000 center 0.000 0.000 0.000 translation -0.000 0.000 0.000 #translation -10.500 0.500 29.500 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 29.500 7.000 -67.002, 31.000 7.000 -68.000, 31.000 7.000 -65.268, 30.000 7.000 -68.000, 30.649 7.000 -65.108, 25.070 9.500 -61.047, 25.089 9.500 -61.024, 25.106 9.500 -61.032, 25.142 9.500 -61.016, 25.177 9.500 -61.000, 23.500 9.500 -65.024, 28.529 9.500 -65.645, 28.296 9.500 -65.953, 28.127 9.500 -66.299, 28.027 9.500 -66.671, 28.001 9.500 -67.055, 28.048 9.500 -67.437, 28.168 9.500 -67.803, 28.356 9.500 -68.140, 28.605 9.500 -68.434, 16.080 9.500 -63.629, 28.529 -7.500 -65.645, 24.946 -7.500 -65.024, 28.816 -7.500 -65.388, 28.296 -7.500 -65.953, 28.127 -7.500 -66.299, 24.871 -7.500 -65.636, 25.089 7.000 -61.024, 25.070 7.000 -61.047, 25.106 7.000 -61.032, 25.142 7.000 -61.016, 25.177 7.000 -61.000, 28.356 7.000 -68.140, 24.310 7.000 -66.956, 28.605 7.000 -68.434, 28.906 7.000 -68.674, 28.168 7.000 -67.803, 29.371 7.000 -69.000, 29.247 7.000 -68.853, 28.048 7.000 -67.437, 29.617 7.000 -68.963, 28.001 7.000 -67.055, 24.871 7.000 -65.636, 30.000 7.000 -69.000, 28.027 7.000 -66.671, 28.127 7.000 -66.299, 28.816 7.000 -65.388, 29.146 7.000 -65.191, 29.509 7.000 -65.061, 29.890 7.000 -65.003, 30.275 7.000 -65.019, 31.000 7.000 -65.047, 24.946 7.000 -65.024, 28.529 7.000 -65.645, 28.296 7.000 -65.953, 20.790 8.500 -65.533, 24.285 8.500 -67.017, 20.790 7.000 -65.533, 20.142 0.500 -65.258, 29.500 -10.000 -67.002, 31.000 -10.000 -68.000, 31.000 -10.000 -65.268, 30.000 -10.000 -68.000, 30.649 -10.000 -65.108, 29.371 -7.500 -69.000, 29.371 -10.000 -69.000, 24.310 -7.500 -66.956, 22.685 -8.750 -66.299, 25.000 7.000 -61.000, 23.055 7.000 -61.858, 20.552 7.000 -62.049, 16.000 7.000 -63.342, 16.044 -7.500 -63.396, 16.000 -7.500 -63.342, 16.044 7.000 -63.396, 2.522 -0.250 -46.698, 25.089 -10.000 -61.024, 25.070 -10.000 -61.047, 25.106 -10.000 -61.032, 25.142 -10.000 -61.016, 25.177 -10.000 -61.000, 25.000 -10.000 -61.000, 28.816 -10.000 -65.388, 29.146 -10.000 -65.191, 28.529 -10.000 -65.645, 29.509 -10.000 -65.061, 29.890 -10.000 -65.003, 30.275 -10.000 -65.019, 31.000 -10.000 -65.047, 23.500 -10.000 -65.024, -11.000 7.000 -30.000, 2.500 7.000 -46.766, 16.000 7.000 -63.500, 16.000 7.000 -63.531, -10.954 7.000 -30.246, 16.000 9.500 -61.000, 7.000 9.500 -46.766, 25.000 9.500 -61.000, 16.000 9.500 -63.531, -10.954 9.500 -30.246, 11.841 9.500 -30.000, 3.645 -0.250 -45.702, 16.033 7.000 -61.000, -9.070 7.000 -30.000, -9.070 -7.500 -30.000, 0.707 -13.500 0.707, 0.866 -13.500 0.500, 6.313 -13.500 4.914, 6.815 -13.500 4.189, 0.966 -13.500 0.259, 7.235 -13.500 3.414, 7.567 -13.500 2.598, 1.000 -13.500 -0.000, 7.951 -13.500 0.880, 8.000 -13.500 0.000, 0.966 -13.500 -0.259, 0.866 -13.500 -0.500, 7.235 -13.500 -3.414, 6.815 -13.500 -4.189, 0.707 -13.500 -0.707, 6.313 -13.500 -4.914, 5.734 -13.500 -5.578, 11.000 -7.500 -30.000, 10.857 -7.500 -29.500, 11.262 -7.500 -29.965, 11.506 -7.500 -29.863, 10.741 -7.500 -29.966, -11.000 9.500 -30.000, 0.000 9.500 -29.500, 11.000 9.500 -30.000, -11.000 9.500 -29.000, 10.857 9.500 -29.500, 11.262 9.500 -29.965, 11.506 9.500 -29.863, 10.741 9.500 -29.966, 10.134 9.500 -29.500, 10.034 9.500 -29.259, 10.293 9.500 -29.707, 10.500 9.500 -29.866, 11.000 9.500 -29.000, -0.000 -7.500 -29.500, 10.000 -7.500 -29.000, 10.034 -7.500 -29.259, 10.134 -7.500 -29.500, 10.293 -7.500 -29.707, 10.500 -7.500 -29.866, -9.834 -7.500 -29.056, 11.000 -7.500 -29.000, 11.500 -7.500 -27.500, 11.417 -7.500 -29.000, 11.000 -7.500 -25.000, 11.841 7.000 -30.000, 12.000 7.000 -30.000, 11.714 7.000 -29.700, 11.417 7.000 -29.000, 11.500 7.000 -27.500, 11.500 9.500 -27.500, 11.417 9.500 -29.000, 11.000 9.500 -25.000, 7.000 -10.000 -46.766, 16.000 -10.000 -61.000, 16.000 -10.000 -63.531, -10.954 -10.000 -30.246, 11.841 -10.000 -30.000, 11.000 -10.000 -30.000, -11.000 8.500 -30.000, -11.000 8.250 -29.500, -11.000 -8.500 -30.000, -11.000 -7.500 -29.000, -11.000 -7.500 -30.000, -11.000 -8.750 -29.500, -11.000 12.000 10.000, -3.464 12.000 2.000, -3.864 12.000 1.035, -4.000 12.000 0.000, -3.864 12.000 -1.035, -11.000 12.000 -29.000, 0.000 12.000 -9.500, 11.000 12.000 -29.000, 0.000 12.000 -19.250, -3.464 12.000 -2.000, -2.828 12.000 -2.828, -2.000 12.000 -3.464, -1.035 12.000 -3.864, 0.000 12.000 -4.000, 1.035 12.000 -3.864, 2.000 12.000 -3.464, 2.828 12.000 -2.828, 3.464 12.000 -2.000, 4.000 12.000 -0.000, 11.000 12.000 10.000, 3.864 12.000 -1.035, 3.864 12.000 1.035, 3.464 12.000 2.000, 2.828 12.000 2.828, 2.000 12.000 3.464, 1.035 12.000 3.864, 0.000 12.000 4.000, 0.000 12.000 7.000, -2.000 12.000 3.464, -1.035 12.000 3.864, 3.612 -12.500 -7.138, 11.000 -12.500 -29.000, 4.376 -12.500 -6.697, 5.086 -12.500 -6.175, 5.734 -12.500 -5.578, 6.313 -12.500 -4.914, 6.815 -12.500 -4.189, 7.235 -12.500 -3.414, 7.567 -12.500 -2.598, -11.000 -12.500 -29.000, -5.418 -12.500 -5.886, -6.033 -12.500 -5.254, -4.738 -12.500 -6.446, -6.574 -12.500 -4.558, -4.000 -12.500 -6.928, -7.036 -12.500 -3.808, -3.214 -12.500 -7.326, -7.412 -12.500 -3.010, -7.698 -12.500 -2.177, -7.891 -12.500 -1.317, -11.000 -12.500 10.000, -7.036 -12.500 3.808, -6.574 -12.500 4.558, -7.412 -12.500 3.010, -7.698 -12.500 2.177, -7.891 -12.500 1.317, -7.988 -12.500 0.441, -7.988 -12.500 -0.441, 11.000 -10.000 -25.000, 11.000 -0.250 -9.500, 11.000 7.000 -25.000, 11.000 7.000 -29.000, 11.000 -0.250 0.250, 11.000 -12.500 10.000, 11.506 -10.000 -29.863, 11.714 -10.000 -29.700, 11.417 -10.000 -29.000, 11.181 -10.000 -29.431, 10.857 -10.000 -29.500, 11.000 -10.000 -29.000, 10.000 -10.000 -29.000, -0.000 -11.250 -29.000, -11.000 -10.000 -29.000, -11.000 -0.250 -9.500, -11.000 -6.375 -9.500, -11.000 -0.250 0.250, -11.000 5.875 -9.500, -11.000 7.000 -29.000, -0.000 -10.000 -29.500, 10.034 -10.000 -29.259, 10.134 -10.000 -29.500, 10.293 -10.000 -29.707, 10.500 -10.000 -29.866, 10.741 -10.000 -29.966, -11.000 -10.000 -30.000, 11.262 -10.000 -29.965, -6.033 -12.500 5.254, -5.418 -12.500 5.886, -4.738 -12.500 6.446, -4.000 -12.500 6.928, -3.214 -12.500 7.326, -2.388 -12.500 7.635, -1.534 -12.500 7.852, -0.661 -12.500 7.973, 0.220 -12.500 7.997, 7.951 -12.500 0.880, 8.000 -12.500 0.000, 7.806 -12.500 1.749, 7.567 -12.500 2.598, 7.235 -12.500 3.414, 6.815 -12.500 4.189, 6.313 -12.500 4.914, 5.734 -12.500 5.578, 5.086 -12.500 6.175, 4.376 -12.500 6.697, 3.612 -12.500 7.138, 2.805 -12.500 7.492, 1.964 -12.500 7.755, 1.099 -12.500 7.924, 7.951 -12.500 -0.880, 7.806 -12.500 -1.749, -0.000 -12.500 -19.250, -0.000 -12.500 -9.500, -2.388 -12.500 -7.635, -1.534 -12.500 -7.852, -0.661 -12.500 -7.973, 0.220 -12.500 -7.997, 1.099 -12.500 -7.924, 1.964 -12.500 -7.755, 2.805 -12.500 -7.492, -2.828 12.000 2.828, 10.000 9.500 -29.000, 0.000 10.750 -29.000, 10.000 7.000 -29.000, -5.500 -0.250 -29.000, -0.000 -0.250 -29.000, -0.500 3.375 -29.000, -0.500 -3.875 -29.000, 5.000 -0.250 -29.000, -11.000 -10.000 -30.283, 11.673 -10.000 -29.850, 12.000 -10.000 -30.000, 11.500 -10.000 -27.500, 12.000 -10.000 -25.000, 12.000 9.500 -30.000, 12.000 9.500 -25.000, 11.714 9.500 -29.700, 12.000 7.000 -25.000, 12.000 -7.500 -30.000, 12.000 -7.500 -25.000, 11.841 -7.500 -30.000, 11.714 -7.500 -29.700, 7.806 -13.500 1.749, 5.734 -13.500 5.578, 5.086 -13.500 6.175, 4.376 -13.500 6.697, 3.612 -13.500 7.138, 2.805 -13.500 7.492, 1.964 -13.500 7.755, 1.099 -13.500 7.924, 0.220 -13.500 7.997, -0.661 -13.500 7.973, -1.534 -13.500 7.852, -2.388 -13.500 7.635, -3.214 -13.500 7.326, -4.000 -13.500 6.928, -4.738 -13.500 6.446, -5.418 -13.500 5.886, -6.033 -13.500 5.254, -6.574 -13.500 4.558, -7.036 -13.500 3.808, -7.412 -13.500 3.010, -7.698 -13.500 2.177, -7.891 -13.500 1.317, -7.988 -13.500 0.441, -7.988 -13.500 -0.441, -7.891 -13.500 -1.317, -7.698 -13.500 -2.177, -7.412 -13.500 -3.010, -7.036 -13.500 -3.808, -6.574 -13.500 -4.558, -6.033 -13.500 -5.254, -5.418 -13.500 -5.886, -4.738 -13.500 -6.446, -4.000 -13.500 -6.928, -3.214 -13.500 -7.326, -2.388 -13.500 -7.635, -1.534 -13.500 -7.852, -0.661 -13.500 -7.973, 0.220 -13.500 -7.997, 1.099 -13.500 -7.924, 1.964 -13.500 -7.755, 2.805 -13.500 -7.492, 3.612 -13.500 -7.138, 4.376 -13.500 -6.697, 5.086 -13.500 -6.175, 7.567 -13.500 -2.598, 7.806 -13.500 -1.749, 7.951 -13.500 -0.880, -0.448 12.500 3.975, 0.000 12.500 4.000, -0.890 12.500 3.900, -1.321 12.500 3.776, -1.736 12.500 3.604, -2.128 12.500 3.387, -2.494 12.500 3.127, -2.828 12.500 2.828, -3.127 12.500 2.494, -3.387 12.500 2.128, -3.604 12.500 1.736, -3.776 12.500 1.321, -3.900 12.500 0.890, -3.975 12.500 0.448, -4.000 12.500 0.000, -3.975 12.500 -0.448, -3.900 12.500 -0.890, -3.776 12.500 -1.321, -3.604 12.500 -1.736, -3.387 12.500 -2.128, -3.127 12.500 -2.494, -2.828 12.500 -2.828, -2.494 12.500 -3.127, -2.128 12.500 -3.387, -1.736 12.500 -3.604, -1.321 12.500 -3.776, -0.890 12.500 -3.900, -0.448 12.500 -3.975, 0.000 12.500 -4.000, 0.448 12.500 -3.975, 0.890 12.500 -3.900, 1.321 12.500 -3.776, 1.736 12.500 -3.604, 2.128 12.500 -3.387, 2.494 12.500 -3.127, 2.828 12.500 -2.828, 3.127 12.500 -2.494, 3.387 12.500 -2.128, 3.604 12.500 -1.736, 3.776 12.500 -1.321, 3.900 12.500 -0.890, 3.975 12.500 -0.448, 4.000 12.500 -0.000, 3.975 12.500 0.448, 3.900 12.500 0.890, 3.776 12.500 1.321, 3.604 12.500 1.736, 3.387 12.500 2.128, 3.127 12.500 2.494, 2.828 12.500 2.828, 2.494 12.500 3.127, 2.128 12.500 3.387, 1.736 12.500 3.604, 1.321 12.500 3.776, 0.890 12.500 3.900, 0.448 12.500 3.975, -11.000 -7.500 -30.283, -11.000 -8.750 -30.141, 11.181 9.500 -29.431, -9.834 7.000 -29.056, 0.000 7.000 -29.500, 10.034 7.000 -29.259, 10.134 7.000 -29.500, 10.293 7.000 -29.707, 10.500 7.000 -29.866, 10.741 7.000 -29.966, 11.000 7.000 -30.000, 11.506 7.000 -29.863, 10.857 7.000 -29.500, 11.262 7.000 -29.965, 11.181 7.000 -29.431, 11.181 -7.500 -29.431, -11.000 9.500 -30.283, -11.000 8.250 -30.141, -11.000 7.000 -30.283, 19.149 -10.000 -63.038, 16.080 -10.000 -63.629, 22.154 -10.000 -62.175, 16.000 -7.500 -63.531, -10.954 -7.500 -30.246, 25.000 -7.500 -61.000, 11.673 9.500 -29.850, 11.673 7.000 -29.850, 11.673 -7.500 -29.850, -0.707 -13.500 -0.707, -0.866 -13.500 -0.500, -0.500 -13.500 -0.866, -0.966 -13.500 -0.259, -0.259 -13.500 -0.966, -1.000 -13.500 -0.000, -0.000 -13.500 -1.000, -0.966 -13.500 0.259, 0.259 -13.500 -0.966, -0.866 -13.500 0.500, 0.500 -13.500 -0.866, -0.707 -13.500 0.707, -0.500 -13.500 0.866, -0.259 -13.500 0.966, -0.000 -13.500 1.000, 0.259 -13.500 0.966, 0.500 -13.500 0.866, 0.749 12.500 -0.663, 0.663 12.500 -0.749, 0.568 12.500 -0.823, 0.823 12.500 -0.568, 0.885 12.500 -0.465, 0.465 12.500 -0.885, 0.935 12.500 -0.355, 0.355 12.500 -0.935, 0.971 12.500 -0.239, 0.239 12.500 -0.971, 0.993 12.500 -0.121, 0.121 12.500 -0.993, 0.000 12.500 -1.000, 1.000 12.500 0.000, -0.121 12.500 -0.993, 0.993 12.500 0.121, -0.239 12.500 -0.971, 0.971 12.500 0.239, -0.355 12.500 -0.935, 0.935 12.500 0.355, -0.465 12.500 -0.885, 0.885 12.500 0.465, -0.568 12.500 -0.823, 0.823 12.500 0.568, -0.663 12.500 -0.749, 0.749 12.500 0.663, -0.749 12.500 -0.663, 0.663 12.500 0.749, 0.568 12.500 0.823, -0.823 12.500 -0.568, 0.465 12.500 0.885, -0.885 12.500 -0.465, 0.355 12.500 0.935, -0.935 12.500 -0.355, -0.971 12.500 -0.239, 0.239 12.500 0.971, 0.121 12.500 0.993, -0.993 12.500 -0.121, 0.000 12.500 1.000, -1.000 12.500 -0.000, -0.993 12.500 0.121, -0.121 12.500 0.993, -0.239 12.500 0.971, -0.971 12.500 0.239, -0.935 12.500 0.355, -0.355 12.500 0.935, -0.885 12.500 0.465, -0.465 12.500 0.885, -0.568 12.500 0.823, -0.823 12.500 0.568, -0.663 12.500 0.749, -0.749 12.500 0.663, 2.500 -7.500 -46.766, 16.000 -7.500 -63.500, 16.033 -7.500 -61.000, 17.125 -7.500 -62.348, 17.125 7.000 -62.348, 16.000 -10.000 -63.598, 28.906 -10.000 -68.674, 28.605 -10.000 -68.434, 29.247 -10.000 -68.853, 28.296 -10.000 -65.953, 28.127 -10.000 -66.299, 28.027 -10.000 -66.671, 28.001 -10.000 -67.055, 28.048 -10.000 -67.437, 28.168 -10.000 -67.803, 28.356 -10.000 -68.140, 29.617 -10.000 -68.963, 30.000 -10.000 -69.000, 16.000 -7.500 -63.598, 25.177 -9.000 -61.000, 25.089 -8.750 -61.000, 25.177 -8.000 -61.000, 25.177 -7.500 -61.000, -0.121 -14.500 -0.993, -0.000 -14.500 -1.000, -0.239 -14.500 -0.971, -0.355 -14.500 -0.935, -0.465 -14.500 -0.885, -0.568 -14.500 -0.823, -0.663 -14.500 -0.749, -0.749 -14.500 -0.663, -0.823 -14.500 -0.568, -0.885 -14.500 -0.465, -0.935 -14.500 -0.355, -0.971 -14.500 -0.239, -0.993 -14.500 -0.121, -1.000 -14.500 -0.000, -0.993 -14.500 0.121, -0.971 -14.500 0.239, -0.935 -14.500 0.355, -0.885 -14.500 0.465, -0.823 -14.500 0.568, -0.749 -14.500 0.663, -0.663 -14.500 0.749, -0.568 -14.500 0.823, -0.465 -14.500 0.885, -0.355 -14.500 0.935, -0.239 -14.500 0.971, -0.121 -14.500 0.993, -0.000 -14.500 1.000, 0.121 -14.500 0.993, 0.239 -14.500 0.971, 0.355 -14.500 0.935, 0.465 -14.500 0.885, 0.568 -14.500 0.823, 0.663 -14.500 0.749, 0.749 -14.500 0.663, 0.823 -14.500 0.568, 0.885 -14.500 0.465, 0.935 -14.500 0.355, 0.971 -14.500 0.239, 0.993 -14.500 0.121, 1.000 -14.500 0.000, 0.993 -14.500 -0.121, 0.971 -14.500 -0.239, 0.935 -14.500 -0.355, 0.885 -14.500 -0.465, 0.823 -14.500 -0.568, 0.749 -14.500 -0.663, 0.663 -14.500 -0.749, 0.568 -14.500 -0.823, 0.465 -14.500 -0.885, 0.355 -14.500 -0.935, 0.239 -14.500 -0.971, 0.121 -14.500 -0.993, -0.121 13.500 -0.993, 0.000 13.500 -1.000, -0.239 13.500 -0.971, -0.355 13.500 -0.935, -0.465 13.500 -0.885, -0.568 13.500 -0.823, -0.663 13.500 -0.749, -0.749 13.500 -0.663, -0.823 13.500 -0.568, -0.885 13.500 -0.465, -0.935 13.500 -0.355, -0.971 13.500 -0.239, -0.993 13.500 -0.121, -1.000 13.500 -0.000, -0.993 13.500 0.121, -0.971 13.500 0.239, -0.935 13.500 0.355, -0.885 13.500 0.465, -0.823 13.500 0.568, -0.749 13.500 0.663, -0.663 13.500 0.749, -0.568 13.500 0.823, -0.465 13.500 0.885, -0.355 13.500 0.935, -0.239 13.500 0.971, -0.121 13.500 0.993, 0.000 13.500 1.000, 0.121 13.500 0.993, 0.239 13.500 0.971, 0.355 13.500 0.935, 0.465 13.500 0.885, 0.568 13.500 0.823, 0.663 13.500 0.749, 0.749 13.500 0.663, 0.823 13.500 0.568, 0.885 13.500 0.465, 0.935 13.500 0.355, 0.971 13.500 0.239, 0.993 13.500 0.121, 1.000 13.500 0.000, 0.993 13.500 -0.121, 0.971 13.500 -0.239, 0.935 13.500 -0.355, 0.885 13.500 -0.465, 0.823 13.500 -0.568, 0.749 13.500 -0.663, 0.663 13.500 -0.749, 0.568 13.500 -0.823, 0.465 13.500 -0.885, 0.355 13.500 -0.935, 0.239 13.500 -0.971, 0.121 13.500 -0.993, 16.123 -7.500 -63.565, 16.246 -7.500 -63.605, 16.080 -7.500 -63.629, 16.135 -7.500 -63.621, 16.191 -7.500 -63.613, 18.893 7.000 -63.098, 20.993 7.000 -62.543, 20.998 -0.250 -63.992, 22.934 -0.250 -64.814, 18.893 -7.500 -63.098, 19.061 -0.250 -63.170, 20.552 -7.500 -62.049, 20.993 -7.500 -62.543, 23.055 -7.500 -61.858, 25.070 -7.500 -61.047, 19.149 9.500 -63.038, 22.154 9.500 -62.175, 16.123 7.000 -63.565, 16.246 7.000 -63.605, 16.080 7.000 -63.629, 16.135 7.000 -63.621, 16.191 7.000 -63.613, 20.790 -7.500 -65.533, 30.000 -7.500 -69.000, 31.000 -7.500 -65.047, 31.000 -7.500 -65.268, 28.035 -8.750 -63.047, 25.070 -9.000 -61.047, 25.070 -8.000 -61.047, 25.106 -9.000 -61.032, 25.142 -9.000 -61.016, 18.395 -7.500 -64.532, 25.106 -8.000 -61.032, 25.142 -8.000 -61.016, 25.106 -7.500 -61.032, 25.142 -7.500 -61.016, 25.089 -7.500 -61.024, 25.089 8.250 -61.000, 25.177 8.000 -61.000, 25.177 9.000 -61.000, -0.000 -14.500 0.000, 0.000 13.500 0.000, 22.213 0.500 -66.137, 24.285 -7.500 -67.017, 24.310 8.500 -66.956, 24.578 0.500 -66.326, 28.906 -7.500 -68.674, 28.356 -7.500 -68.140, 28.605 -7.500 -68.434, 28.168 -7.500 -67.803, 29.247 -7.500 -68.853, 30.649 -7.500 -65.108, 28.048 -7.500 -67.437, 30.275 -7.500 -65.019, 29.146 -7.500 -65.191, 29.509 -7.500 -65.061, 29.890 -7.500 -65.003, 29.617 -7.500 -68.963, 28.001 -7.500 -67.055, 28.027 -7.500 -66.671, 16.000 9.500 -63.598, 29.371 9.500 -69.000, 28.906 9.500 -68.674, 29.247 9.500 -68.853, 30.649 9.500 -65.108, 31.000 9.500 -65.047, 31.000 9.500 -65.268, 30.275 9.500 -65.019, 28.816 9.500 -65.388, 29.146 9.500 -65.191, 29.509 9.500 -65.061, 29.890 9.500 -65.003, 29.617 9.500 -68.963, 30.000 9.500 -69.000, 16.000 7.000 -63.598, 18.395 7.000 -64.532, 30.390 -10.000 -68.962, 30.390 -7.500 -68.962, 30.765 -10.000 -68.848, 30.765 -7.500 -68.848, 31.111 -10.000 -68.663, 31.111 -7.500 -68.663, 31.414 -10.000 -68.414, 31.414 -7.500 -68.414, 31.663 -10.000 -68.111, 31.663 -7.500 -68.111, 31.848 -10.000 -67.765, 31.848 -7.500 -67.765, 31.962 -10.000 -67.390, 31.962 -7.500 -67.390, 32.000 -7.500 -67.000, 32.000 -10.000 -67.000, 31.956 -10.000 -66.584, 31.827 -10.000 -66.187, 31.618 -10.000 -65.824, 31.338 -10.000 -65.514, 31.956 -7.500 -66.584, 31.827 -7.500 -66.187, 31.618 -7.500 -65.824, 31.338 -7.500 -65.514, 25.070 8.000 -61.047, 25.106 8.000 -61.032, 25.142 8.000 -61.016, 25.106 9.000 -61.032, 25.070 9.000 -61.047, 25.142 9.000 -61.016, 28.035 8.250 -63.047, 29.500 -7.500 -67.002, 31.000 -7.500 -68.000, 30.000 -7.500 -68.000, 29.500 9.500 -67.002, 31.000 9.500 -68.000, 30.000 9.500 -68.000, 30.390 7.000 -68.962, 30.390 9.500 -68.962, 30.765 7.000 -68.848, 30.765 9.500 -68.848, 31.111 7.000 -68.663, 31.111 9.500 -68.663, 31.414 7.000 -68.414, 31.414 9.500 -68.414, 31.663 7.000 -68.111, 31.663 9.500 -68.111, 31.848 7.000 -67.765, 31.848 9.500 -67.765, 31.962 7.000 -67.390, 31.962 9.500 -67.390, 32.000 9.500 -67.000, 32.000 7.000 -67.000, 31.956 7.000 -66.584, 31.827 7.000 -66.187, 31.618 7.000 -65.824, 31.338 7.000 -65.514, 31.956 9.500 -66.584, 31.827 9.500 -66.187, 31.618 9.500 -65.824, 31.338 9.500 -65.514 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 0, 3, 1, -1, 4, 0, 2, -1, 5, 6, 7, -1, 7, 6, 8, -1, 6, 9, 8, -1, 10, 11, 12, -1, 10, 12, 13, -1, 10, 13, 14, -1, 10, 14, 15, -1, 10, 15, 16, -1, 10, 16, 17, -1, 10, 17, 18, -1, 19, 10, 18, -1, 20, 10, 19, -1, 21, 22, 23, -1, 22, 21, 24, -1, 22, 24, 25, -1, 26, 22, 25, -1, 27, 28, 29, -1, 27, 29, 30, -1, 31, 27, 30, -1, 32, 33, 34, -1, 35, 34, 33, -1, 33, 32, 36, -1, 37, 35, 33, -1, 38, 35, 37, -1, 33, 36, 39, -1, 38, 37, 40, -1, 33, 39, 41, -1, 42, 33, 41, -1, 43, 40, 37, -1, 42, 41, 44, -1, 42, 44, 45, -1, 46, 47, 28, -1, 28, 47, 48, -1, 28, 48, 49, -1, 28, 49, 50, -1, 28, 50, 51, -1, 4, 2, 51, -1, 51, 50, 4, -1, 52, 46, 28, -1, 52, 53, 46, -1, 52, 54, 53, -1, 52, 45, 54, -1, 52, 42, 45, -1, 55, 56, 57, -1, 58, 57, 56, -1, 59, 60, 61, -1, 59, 62, 60, -1, 63, 59, 61, -1, 64, 65, 66, -1, 66, 65, 67, -1, 68, 69, 28, -1, 70, 69, 68, -1, 71, 72, 73, -1, 71, 74, 72, -1, 75, 71, 73, -1, 76, 77, 78, -1, 76, 78, 79, -1, 80, 76, 79, -1, 81, 76, 80, -1, 81, 77, 76, -1, 82, 83, 77, -1, 84, 82, 77, -1, 77, 83, 85, -1, 77, 85, 86, -1, 77, 86, 87, -1, 77, 87, 88, -1, 63, 61, 88, -1, 88, 87, 63, -1, 89, 84, 77, -1, 90, 91, 71, -1, 92, 71, 91, -1, 93, 92, 91, -1, 93, 91, 94, -1, 94, 91, 90, -1, 95, 96, 97, -1, 96, 95, 98, -1, 96, 98, 99, -1, 96, 100, 97, -1, 101, 102, 103, -1, 104, 101, 103, -1, 105, 106, 107, -1, 106, 108, 107, -1, 106, 109, 110, -1, 109, 111, 110, -1, 109, 112, 113, -1, 112, 114, 113, -1, 115, 116, 117, -1, 116, 118, 117, -1, 116, 119, 120, -1, 119, 121, 120, -1, 122, 123, 124, -1, 124, 123, 125, -1, 126, 123, 122, -1, 127, 128, 129, -1, 127, 130, 128, -1, 129, 131, 132, -1, 132, 131, 133, -1, 134, 131, 129, -1, 135, 136, 131, -1, 137, 135, 131, -1, 137, 131, 138, -1, 138, 131, 134, -1, 136, 139, 131, -1, 140, 141, 142, -1, 140, 142, 143, -1, 140, 143, 144, -1, 145, 140, 144, -1, 126, 140, 145, -1, 122, 140, 126, -1, 104, 146, 140, -1, 104, 140, 122, -1, 141, 140, 146, -1, 147, 148, 149, -1, 150, 148, 147, -1, 151, 152, 153, -1, 154, 153, 152, -1, 155, 154, 152, -1, 139, 156, 157, -1, 158, 156, 139, -1, 159, 160, 81, -1, 161, 160, 159, -1, 161, 159, 162, -1, 159, 81, 163, -1, 164, 159, 163, -1, 162, 159, 164, -1, 165, 130, 127, -1, 165, 166, 130, -1, 167, 168, 169, -1, 167, 170, 168, -1, 171, 172, 173, -1, 171, 173, 174, -1, 171, 174, 175, -1, 171, 175, 176, -1, 177, 178, 179, -1, 178, 176, 179, -1, 176, 177, 179, -1, 180, 177, 176, -1, 175, 180, 176, -1, 181, 177, 180, -1, 181, 182, 177, -1, 182, 183, 177, -1, 183, 184, 177, -1, 184, 185, 177, -1, 185, 186, 177, -1, 187, 177, 186, -1, 188, 177, 187, -1, 177, 188, 178, -1, 189, 190, 191, -1, 192, 190, 189, -1, 193, 190, 192, -1, 194, 190, 193, -1, 194, 195, 190, -1, 195, 196, 190, -1, 196, 197, 190, -1, 190, 197, 198, -1, 197, 171, 198, -1, 171, 190, 198, -1, 199, 171, 200, -1, 200, 171, 197, -1, 201, 202, 203, -1, 203, 202, 204, -1, 204, 202, 205, -1, 205, 202, 206, -1, 207, 206, 202, -1, 208, 207, 202, -1, 209, 208, 202, -1, 210, 211, 212, -1, 211, 210, 213, -1, 210, 212, 214, -1, 213, 210, 215, -1, 210, 214, 216, -1, 215, 210, 217, -1, 210, 216, 218, -1, 210, 218, 219, -1, 210, 219, 220, -1, 210, 220, 221, -1, 222, 223, 221, -1, 224, 222, 221, -1, 225, 224, 221, -1, 226, 225, 221, -1, 227, 226, 221, -1, 228, 227, 221, -1, 221, 220, 228, -1, 229, 150, 230, -1, 150, 231, 230, -1, 150, 232, 231, -1, 150, 147, 232, -1, 231, 158, 230, -1, 230, 158, 190, -1, 139, 178, 158, -1, 178, 190, 158, -1, 230, 190, 233, -1, 190, 234, 233, -1, 234, 230, 233, -1, 229, 230, 234, -1, 235, 236, 237, -1, 235, 237, 238, -1, 237, 239, 238, -1, 239, 235, 238, -1, 240, 202, 241, -1, 241, 202, 242, -1, 242, 210, 243, -1, 242, 243, 241, -1, 242, 202, 210, -1, 210, 244, 243, -1, 210, 221, 245, -1, 221, 244, 245, -1, 244, 210, 245, -1, 221, 171, 246, -1, 171, 244, 246, -1, 244, 221, 246, -1, 171, 176, 247, -1, 176, 244, 247, -1, 244, 171, 247, -1, 243, 244, 168, -1, 130, 244, 176, -1, 168, 244, 248, -1, 248, 244, 130, -1, 249, 241, 243, -1, 249, 250, 241, -1, 249, 251, 250, -1, 249, 252, 251, -1, 249, 253, 252, -1, 249, 254, 253, -1, 249, 164, 254, -1, 249, 255, 164, -1, 243, 255, 249, -1, 241, 250, 240, -1, 250, 251, 239, -1, 251, 252, 239, -1, 252, 253, 239, -1, 253, 254, 239, -1, 254, 164, 239, -1, 164, 256, 239, -1, 239, 256, 235, -1, 250, 239, 240, -1, 240, 239, 237, -1, 202, 240, 229, -1, 202, 229, 234, -1, 221, 223, 257, -1, 221, 257, 258, -1, 221, 258, 259, -1, 221, 259, 260, -1, 221, 260, 261, -1, 221, 261, 262, -1, 221, 262, 263, -1, 264, 221, 263, -1, 265, 221, 264, -1, 234, 221, 265, -1, 234, 266, 267, -1, 234, 268, 266, -1, 234, 269, 268, -1, 270, 269, 234, -1, 271, 270, 234, -1, 272, 271, 234, -1, 273, 272, 234, -1, 274, 273, 234, -1, 275, 274, 234, -1, 276, 275, 234, -1, 277, 276, 234, -1, 234, 278, 277, -1, 234, 279, 278, -1, 234, 265, 279, -1, 234, 267, 202, -1, 267, 280, 202, -1, 280, 281, 202, -1, 281, 209, 202, -1, 210, 202, 282, -1, 202, 283, 282, -1, 283, 210, 282, -1, 283, 202, 201, -1, 283, 284, 217, -1, 284, 283, 285, -1, 285, 283, 286, -1, 286, 283, 287, -1, 287, 283, 288, -1, 288, 283, 289, -1, 283, 290, 289, -1, 283, 201, 290, -1, 217, 210, 283, -1, 190, 171, 221, -1, 234, 190, 221, -1, 191, 190, 178, -1, 188, 191, 178, -1, 171, 291, 172, -1, 291, 171, 199, -1, 255, 243, 170, -1, 255, 170, 167, -1, 170, 243, 168, -1, 139, 292, 178, -1, 293, 130, 176, -1, 293, 176, 178, -1, 293, 292, 130, -1, 178, 292, 293, -1, 147, 141, 232, -1, 232, 141, 294, -1, 168, 248, 295, -1, 248, 296, 295, -1, 296, 168, 295, -1, 248, 294, 297, -1, 294, 296, 297, -1, 296, 248, 297, -1, 141, 168, 298, -1, 168, 296, 298, -1, 296, 141, 298, -1, 294, 141, 299, -1, 141, 296, 299, -1, 296, 294, 299, -1, 166, 248, 130, -1, 90, 248, 166, -1, 90, 166, 165, -1, 255, 162, 164, -1, 300, 162, 255, -1, 235, 256, 163, -1, 235, 163, 301, -1, 163, 236, 301, -1, 236, 235, 301, -1, 164, 163, 256, -1, 163, 302, 236, -1, 237, 236, 302, -1, 240, 303, 229, -1, 303, 302, 304, -1, 229, 303, 304, -1, 240, 237, 303, -1, 303, 237, 302, -1, 305, 156, 306, -1, 158, 306, 156, -1, 157, 156, 305, -1, 100, 307, 305, -1, 307, 157, 305, -1, 308, 158, 231, -1, 306, 158, 308, -1, 231, 155, 308, -1, 155, 152, 308, -1, 232, 155, 231, -1, 232, 154, 155, -1, 309, 148, 310, -1, 150, 310, 148, -1, 149, 148, 309, -1, 311, 312, 309, -1, 312, 149, 309, -1, 304, 150, 229, -1, 310, 150, 304, -1, 267, 266, 113, -1, 113, 114, 267, -1, 113, 266, 268, -1, 313, 113, 268, -1, 313, 268, 269, -1, 111, 313, 269, -1, 111, 269, 270, -1, 270, 110, 111, -1, 110, 270, 271, -1, 271, 108, 110, -1, 108, 271, 272, -1, 107, 108, 272, -1, 107, 272, 273, -1, 314, 107, 273, -1, 314, 273, 274, -1, 274, 315, 314, -1, 274, 275, 315, -1, 275, 316, 315, -1, 316, 275, 276, -1, 317, 316, 276, -1, 317, 276, 277, -1, 318, 317, 277, -1, 277, 278, 318, -1, 278, 319, 318, -1, 278, 279, 319, -1, 279, 320, 319, -1, 279, 265, 321, -1, 321, 320, 279, -1, 265, 264, 321, -1, 322, 321, 264, -1, 264, 263, 323, -1, 323, 322, 264, -1, 263, 262, 323, -1, 324, 323, 262, -1, 262, 261, 325, -1, 262, 325, 324, -1, 325, 261, 260, -1, 326, 325, 260, -1, 260, 259, 327, -1, 327, 326, 260, -1, 327, 259, 258, -1, 328, 327, 258, -1, 258, 329, 328, -1, 329, 258, 257, -1, 329, 257, 223, -1, 330, 329, 223, -1, 330, 223, 222, -1, 331, 330, 222, -1, 331, 222, 224, -1, 332, 331, 224, -1, 332, 224, 225, -1, 225, 333, 332, -1, 334, 333, 225, -1, 334, 225, 226, -1, 334, 226, 227, -1, 227, 335, 334, -1, 335, 227, 228, -1, 336, 335, 228, -1, 336, 228, 220, -1, 220, 337, 336, -1, 337, 220, 219, -1, 219, 338, 337, -1, 219, 218, 339, -1, 219, 339, 338, -1, 339, 218, 216, -1, 340, 339, 216, -1, 216, 214, 340, -1, 341, 340, 214, -1, 214, 212, 341, -1, 342, 341, 212, -1, 343, 212, 211, -1, 343, 342, 212, -1, 343, 211, 213, -1, 213, 344, 343, -1, 213, 215, 344, -1, 215, 345, 344, -1, 346, 215, 217, -1, 346, 345, 215, -1, 346, 217, 284, -1, 284, 347, 346, -1, 284, 285, 347, -1, 348, 347, 285, -1, 285, 286, 348, -1, 286, 349, 348, -1, 350, 286, 287, -1, 350, 349, 286, -1, 350, 287, 288, -1, 288, 351, 350, -1, 288, 289, 351, -1, 289, 352, 351, -1, 289, 290, 352, -1, 290, 353, 352, -1, 290, 201, 353, -1, 201, 354, 353, -1, 201, 203, 354, -1, 355, 354, 203, -1, 355, 203, 204, -1, 204, 356, 355, -1, 204, 205, 356, -1, 205, 121, 356, -1, 121, 205, 206, -1, 120, 121, 206, -1, 120, 206, 207, -1, 207, 118, 120, -1, 118, 207, 208, -1, 208, 117, 118, -1, 357, 117, 208, -1, 357, 208, 209, -1, 357, 209, 281, -1, 358, 357, 281, -1, 281, 280, 358, -1, 359, 358, 280, -1, 280, 267, 359, -1, 114, 359, 267, -1, 360, 200, 197, -1, 361, 360, 197, -1, 200, 360, 362, -1, 362, 363, 200, -1, 363, 364, 200, -1, 199, 200, 364, -1, 364, 365, 199, -1, 365, 366, 199, -1, 366, 291, 199, -1, 291, 366, 367, -1, 367, 368, 291, -1, 291, 368, 172, -1, 368, 369, 172, -1, 172, 369, 370, -1, 370, 371, 172, -1, 173, 172, 371, -1, 371, 372, 173, -1, 173, 372, 373, -1, 173, 373, 174, -1, 174, 373, 374, -1, 174, 374, 375, -1, 174, 375, 175, -1, 175, 375, 376, -1, 376, 377, 175, -1, 377, 180, 175, -1, 180, 377, 378, -1, 180, 378, 379, -1, 180, 379, 380, -1, 181, 180, 380, -1, 380, 381, 181, -1, 181, 381, 382, -1, 182, 181, 382, -1, 182, 382, 383, -1, 383, 384, 182, -1, 384, 183, 182, -1, 385, 183, 384, -1, 183, 385, 386, -1, 386, 387, 183, -1, 387, 184, 183, -1, 387, 388, 184, -1, 388, 389, 184, -1, 389, 185, 184, -1, 185, 389, 390, -1, 390, 391, 185, -1, 391, 392, 185, -1, 186, 185, 392, -1, 392, 393, 186, -1, 393, 394, 186, -1, 394, 187, 186, -1, 187, 394, 395, -1, 395, 396, 187, -1, 187, 396, 188, -1, 396, 397, 188, -1, 188, 397, 398, -1, 398, 399, 188, -1, 191, 188, 399, -1, 399, 400, 191, -1, 191, 400, 401, -1, 189, 191, 401, -1, 189, 401, 402, -1, 402, 403, 189, -1, 403, 192, 189, -1, 192, 403, 404, -1, 404, 405, 192, -1, 405, 193, 192, -1, 193, 405, 406, -1, 193, 406, 407, -1, 193, 407, 408, -1, 194, 193, 408, -1, 408, 409, 194, -1, 194, 409, 410, -1, 195, 194, 410, -1, 195, 410, 411, -1, 411, 412, 195, -1, 413, 195, 412, -1, 413, 196, 195, -1, 196, 413, 414, -1, 414, 415, 196, -1, 415, 197, 196, -1, 197, 415, 361, -1, 167, 169, 416, -1, 417, 255, 167, -1, 300, 255, 417, -1, 417, 416, 300, -1, 417, 167, 416, -1, 169, 168, 146, -1, 146, 168, 141, -1, 136, 292, 139, -1, 133, 157, 307, -1, 131, 139, 157, -1, 131, 157, 418, -1, 157, 133, 418, -1, 133, 131, 418, -1, 128, 292, 136, -1, 128, 136, 135, -1, 128, 135, 137, -1, 138, 128, 137, -1, 134, 128, 138, -1, 129, 128, 134, -1, 292, 128, 130, -1, 248, 90, 419, -1, 248, 419, 294, -1, 420, 294, 419, -1, 420, 421, 294, -1, 420, 422, 421, -1, 420, 423, 422, -1, 420, 424, 423, -1, 420, 425, 424, -1, 420, 426, 425, -1, 419, 103, 420, -1, 420, 103, 426, -1, 294, 421, 232, -1, 427, 153, 154, -1, 421, 422, 428, -1, 422, 423, 428, -1, 423, 424, 428, -1, 424, 425, 428, -1, 425, 426, 428, -1, 426, 429, 428, -1, 428, 429, 427, -1, 232, 428, 154, -1, 421, 428, 232, -1, 427, 154, 430, -1, 154, 428, 430, -1, 428, 427, 430, -1, 142, 141, 147, -1, 125, 149, 312, -1, 143, 142, 123, -1, 144, 143, 123, -1, 144, 123, 145, -1, 145, 123, 126, -1, 123, 147, 149, -1, 142, 147, 123, -1, 123, 149, 431, -1, 149, 125, 431, -1, 125, 123, 431, -1, 165, 127, 432, -1, 433, 90, 165, -1, 434, 90, 433, -1, 433, 432, 434, -1, 433, 165, 432, -1, 160, 161, 435, -1, 161, 436, 435, -1, 160, 435, 437, -1, 81, 160, 437, -1, 81, 437, 77, -1, 161, 162, 438, -1, 162, 439, 438, -1, 416, 439, 162, -1, 416, 162, 300, -1, 163, 81, 311, -1, 81, 440, 311, -1, 302, 309, 304, -1, 304, 309, 310, -1, 309, 163, 311, -1, 302, 163, 309, -1, 305, 151, 100, -1, 152, 151, 305, -1, 132, 133, 100, -1, 307, 100, 441, -1, 100, 133, 441, -1, 133, 307, 441, -1, 132, 100, 129, -1, 152, 305, 308, -1, 308, 305, 306, -1, 427, 429, 151, -1, 427, 151, 442, -1, 151, 153, 442, -1, 153, 427, 442, -1, 426, 151, 429, -1, 124, 125, 311, -1, 312, 311, 443, -1, 311, 125, 443, -1, 125, 312, 443, -1, 124, 311, 122, -1, 341, 342, 444, -1, 342, 343, 444, -1, 343, 344, 444, -1, 339, 340, 445, -1, 340, 341, 445, -1, 341, 444, 445, -1, 344, 345, 446, -1, 345, 346, 446, -1, 444, 344, 446, -1, 339, 445, 447, -1, 337, 338, 447, -1, 338, 339, 447, -1, 346, 347, 448, -1, 347, 348, 448, -1, 348, 349, 448, -1, 446, 346, 448, -1, 337, 447, 449, -1, 334, 335, 449, -1, 335, 336, 449, -1, 336, 337, 449, -1, 349, 350, 450, -1, 350, 351, 450, -1, 448, 349, 450, -1, 334, 449, 451, -1, 332, 333, 451, -1, 333, 334, 451, -1, 351, 352, 452, -1, 352, 353, 452, -1, 450, 351, 452, -1, 330, 331, 453, -1, 331, 332, 453, -1, 332, 451, 453, -1, 353, 354, 454, -1, 354, 355, 454, -1, 355, 356, 454, -1, 452, 353, 454, -1, 330, 453, 455, -1, 329, 330, 455, -1, 454, 356, 119, -1, 119, 356, 121, -1, 329, 455, 328, -1, 328, 455, 327, -1, 455, 456, 327, -1, 116, 120, 118, -1, 327, 456, 326, -1, 326, 456, 325, -1, 456, 457, 325, -1, 115, 117, 357, -1, 325, 457, 324, -1, 115, 357, 358, -1, 324, 457, 323, -1, 115, 358, 359, -1, 112, 115, 359, -1, 323, 457, 322, -1, 457, 458, 322, -1, 112, 359, 114, -1, 322, 458, 321, -1, 321, 458, 320, -1, 458, 459, 320, -1, 109, 113, 313, -1, 320, 459, 319, -1, 109, 313, 111, -1, 319, 459, 318, -1, 459, 460, 318, -1, 318, 460, 317, -1, 106, 110, 108, -1, 317, 460, 316, -1, 316, 460, 315, -1, 460, 105, 315, -1, 105, 107, 314, -1, 315, 105, 314, -1, 461, 396, 395, -1, 394, 462, 395, -1, 462, 461, 395, -1, 393, 463, 394, -1, 463, 462, 394, -1, 461, 464, 396, -1, 464, 397, 396, -1, 465, 398, 397, -1, 464, 465, 397, -1, 392, 466, 393, -1, 466, 463, 393, -1, 467, 399, 398, -1, 465, 467, 398, -1, 468, 466, 392, -1, 391, 468, 392, -1, 469, 400, 399, -1, 467, 469, 399, -1, 390, 470, 391, -1, 470, 468, 391, -1, 469, 471, 400, -1, 471, 401, 400, -1, 389, 472, 390, -1, 472, 470, 390, -1, 387, 473, 388, -1, 388, 473, 389, -1, 473, 472, 389, -1, 474, 402, 401, -1, 474, 403, 402, -1, 471, 474, 401, -1, 386, 475, 387, -1, 475, 473, 387, -1, 474, 476, 403, -1, 476, 404, 403, -1, 385, 477, 386, -1, 477, 475, 386, -1, 478, 405, 404, -1, 476, 478, 404, -1, 384, 479, 385, -1, 479, 477, 385, -1, 480, 406, 405, -1, 478, 480, 405, -1, 481, 479, 384, -1, 383, 481, 384, -1, 482, 407, 406, -1, 480, 482, 406, -1, 483, 481, 383, -1, 382, 483, 383, -1, 482, 484, 407, -1, 484, 408, 407, -1, 485, 483, 382, -1, 484, 486, 408, -1, 486, 409, 408, -1, 381, 485, 382, -1, 381, 487, 485, -1, 488, 409, 486, -1, 488, 410, 409, -1, 489, 410, 488, -1, 380, 487, 381, -1, 380, 490, 487, -1, 411, 410, 489, -1, 491, 411, 489, -1, 380, 379, 490, -1, 379, 492, 490, -1, 412, 411, 491, -1, 493, 412, 491, -1, 379, 378, 492, -1, 378, 494, 492, -1, 378, 377, 494, -1, 377, 495, 494, -1, 413, 412, 493, -1, 496, 413, 493, -1, 414, 413, 496, -1, 497, 414, 496, -1, 377, 376, 495, -1, 376, 498, 495, -1, 415, 414, 497, -1, 499, 415, 497, -1, 376, 375, 498, -1, 375, 500, 498, -1, 375, 374, 500, -1, 361, 415, 499, -1, 374, 373, 500, -1, 373, 501, 500, -1, 360, 361, 499, -1, 502, 360, 499, -1, 362, 360, 502, -1, 503, 362, 502, -1, 373, 372, 501, -1, 372, 504, 501, -1, 372, 371, 504, -1, 371, 505, 504, -1, 363, 362, 503, -1, 506, 363, 503, -1, 371, 370, 505, -1, 370, 507, 505, -1, 364, 363, 506, -1, 508, 364, 506, -1, 365, 364, 508, -1, 509, 365, 508, -1, 370, 369, 507, -1, 369, 510, 507, -1, 366, 365, 509, -1, 511, 366, 509, -1, 369, 368, 510, -1, 368, 512, 510, -1, 367, 366, 511, -1, 368, 367, 512, -1, 512, 367, 511, -1, 416, 169, 439, -1, 513, 438, 439, -1, 513, 169, 73, -1, 513, 73, 514, -1, 513, 514, 438, -1, 513, 439, 169, -1, 146, 104, 103, -1, 103, 419, 146, -1, 515, 101, 104, -1, 102, 101, 515, -1, 102, 515, 516, -1, 516, 517, 102, -1, 104, 122, 515, -1, 515, 311, 440, -1, 122, 311, 515, -1, 419, 90, 146, -1, 169, 146, 90, -1, 432, 127, 99, -1, 99, 127, 129, -1, 129, 100, 96, -1, 99, 129, 96, -1, 103, 102, 426, -1, 426, 102, 151, -1, 102, 68, 151, -1, 434, 94, 90, -1, 432, 99, 94, -1, 432, 94, 434, -1, 518, 436, 161, -1, 65, 436, 518, -1, 65, 519, 436, -1, 519, 520, 436, -1, 521, 519, 65, -1, 435, 436, 89, -1, 435, 89, 437, -1, 437, 89, 77, -1, 89, 436, 520, -1, 89, 522, 84, -1, 89, 523, 522, -1, 89, 524, 523, -1, 89, 525, 524, -1, 89, 526, 525, -1, 89, 527, 526, -1, 89, 528, 527, -1, 89, 520, 528, -1, 521, 65, 529, -1, 530, 529, 65, -1, 161, 438, 531, -1, 518, 161, 531, -1, 80, 532, 81, -1, 81, 533, 440, -1, 532, 533, 81, -1, 534, 533, 532, -1, 440, 533, 534, -1, 535, 440, 534, -1, 97, 100, 68, -1, 151, 68, 100, -1, 448, 450, 536, -1, 450, 537, 536, -1, 448, 536, 538, -1, 538, 539, 448, -1, 539, 446, 448, -1, 539, 540, 446, -1, 446, 540, 541, -1, 541, 444, 446, -1, 541, 542, 444, -1, 542, 543, 444, -1, 543, 544, 444, -1, 445, 444, 544, -1, 445, 544, 545, -1, 545, 546, 445, -1, 546, 447, 445, -1, 546, 547, 447, -1, 547, 548, 447, -1, 548, 449, 447, -1, 449, 548, 549, -1, 549, 550, 449, -1, 550, 451, 449, -1, 550, 551, 451, -1, 451, 551, 552, -1, 453, 451, 552, -1, 453, 552, 553, -1, 553, 554, 453, -1, 554, 455, 453, -1, 455, 554, 555, -1, 555, 556, 455, -1, 455, 556, 557, -1, 456, 455, 557, -1, 557, 558, 456, -1, 456, 558, 559, -1, 457, 456, 559, -1, 457, 559, 560, -1, 457, 560, 561, -1, 561, 458, 457, -1, 458, 561, 562, -1, 562, 563, 458, -1, 459, 458, 563, -1, 459, 563, 564, -1, 564, 565, 459, -1, 565, 460, 459, -1, 565, 566, 460, -1, 460, 566, 567, -1, 567, 105, 460, -1, 567, 568, 105, -1, 568, 569, 105, -1, 569, 570, 105, -1, 106, 105, 570, -1, 106, 570, 571, -1, 571, 572, 106, -1, 572, 109, 106, -1, 572, 573, 109, -1, 573, 574, 109, -1, 574, 112, 109, -1, 574, 575, 112, -1, 112, 575, 576, -1, 115, 112, 576, -1, 576, 577, 115, -1, 115, 577, 578, -1, 116, 115, 578, -1, 116, 578, 579, -1, 579, 580, 116, -1, 580, 119, 116, -1, 119, 580, 581, -1, 581, 582, 119, -1, 119, 582, 583, -1, 454, 119, 583, -1, 583, 584, 454, -1, 454, 584, 585, -1, 452, 454, 585, -1, 452, 585, 586, -1, 452, 586, 587, -1, 587, 450, 452, -1, 587, 537, 450, -1, 588, 473, 475, -1, 588, 589, 473, -1, 590, 475, 477, -1, 475, 590, 588, -1, 477, 479, 591, -1, 477, 591, 590, -1, 592, 479, 481, -1, 479, 592, 591, -1, 481, 483, 593, -1, 481, 593, 592, -1, 483, 485, 594, -1, 594, 593, 483, -1, 595, 485, 487, -1, 595, 594, 485, -1, 487, 490, 596, -1, 596, 595, 487, -1, 490, 492, 597, -1, 597, 596, 490, -1, 492, 494, 598, -1, 492, 598, 597, -1, 599, 494, 495, -1, 599, 598, 494, -1, 600, 495, 498, -1, 495, 600, 599, -1, 601, 498, 500, -1, 498, 601, 600, -1, 602, 500, 501, -1, 602, 601, 500, -1, 501, 504, 603, -1, 501, 603, 602, -1, 604, 504, 505, -1, 604, 603, 504, -1, 505, 507, 605, -1, 605, 604, 505, -1, 606, 507, 510, -1, 606, 605, 507, -1, 607, 510, 512, -1, 607, 606, 510, -1, 512, 511, 608, -1, 608, 607, 512, -1, 609, 511, 509, -1, 511, 609, 608, -1, 610, 509, 508, -1, 509, 610, 609, -1, 611, 508, 506, -1, 508, 611, 610, -1, 506, 612, 611, -1, 506, 503, 612, -1, 503, 613, 612, -1, 503, 502, 613, -1, 502, 614, 613, -1, 502, 499, 614, -1, 499, 615, 614, -1, 499, 497, 615, -1, 497, 616, 615, -1, 616, 497, 496, -1, 496, 617, 616, -1, 496, 493, 617, -1, 493, 618, 617, -1, 618, 493, 491, -1, 491, 619, 618, -1, 491, 489, 619, -1, 489, 488, 620, -1, 620, 619, 489, -1, 621, 488, 486, -1, 621, 620, 488, -1, 486, 484, 622, -1, 622, 621, 486, -1, 484, 482, 623, -1, 623, 622, 484, -1, 482, 480, 624, -1, 482, 624, 623, -1, 625, 480, 478, -1, 625, 624, 480, -1, 626, 478, 476, -1, 478, 626, 625, -1, 476, 474, 627, -1, 476, 627, 626, -1, 628, 474, 471, -1, 628, 627, 474, -1, 471, 469, 629, -1, 471, 629, 628, -1, 630, 469, 467, -1, 630, 629, 469, -1, 467, 465, 631, -1, 631, 630, 467, -1, 632, 465, 464, -1, 632, 631, 465, -1, 633, 464, 461, -1, 633, 632, 464, -1, 461, 462, 634, -1, 634, 633, 461, -1, 635, 462, 463, -1, 462, 635, 634, -1, 636, 463, 466, -1, 463, 636, 635, -1, 466, 637, 636, -1, 637, 466, 468, -1, 468, 638, 637, -1, 468, 470, 638, -1, 470, 639, 638, -1, 470, 472, 639, -1, 589, 639, 472, -1, 472, 473, 589, -1, 169, 75, 73, -1, 90, 75, 169, -1, 75, 90, 71, -1, 73, 72, 514, -1, 438, 514, 640, -1, 641, 640, 514, -1, 438, 640, 642, -1, 642, 640, 643, -1, 643, 640, 644, -1, 644, 640, 641, -1, 102, 517, 70, -1, 517, 645, 70, -1, 645, 646, 70, -1, 70, 646, 69, -1, 70, 68, 102, -1, 647, 645, 517, -1, 42, 645, 647, -1, 647, 26, 648, -1, 26, 42, 648, -1, 42, 647, 648, -1, 647, 649, 26, -1, 516, 649, 647, -1, 647, 517, 650, -1, 517, 516, 650, -1, 516, 647, 650, -1, 516, 515, 651, -1, 649, 516, 651, -1, 649, 651, 652, -1, 652, 651, 653, -1, 440, 651, 515, -1, 653, 651, 440, -1, 653, 440, 654, -1, 98, 95, 655, -1, 20, 98, 655, -1, 655, 95, 656, -1, 95, 97, 656, -1, 656, 97, 5, -1, 93, 94, 98, -1, 94, 99, 98, -1, 92, 74, 71, -1, 92, 93, 657, -1, 657, 658, 92, -1, 93, 659, 657, -1, 659, 660, 657, -1, 657, 660, 661, -1, 657, 661, 658, -1, 662, 518, 531, -1, 65, 518, 67, -1, 67, 518, 662, -1, 66, 67, 662, -1, 663, 65, 64, -1, 530, 65, 663, -1, 87, 59, 63, -1, 86, 59, 87, -1, 85, 59, 86, -1, 83, 59, 85, -1, 82, 59, 83, -1, 84, 59, 82, -1, 522, 59, 84, -1, 522, 523, 59, -1, 523, 524, 59, -1, 524, 525, 59, -1, 525, 526, 59, -1, 526, 527, 59, -1, 527, 528, 59, -1, 528, 520, 59, -1, 520, 62, 59, -1, 520, 519, 62, -1, 519, 521, 62, -1, 521, 529, 62, -1, 529, 530, 62, -1, 88, 61, 664, -1, 61, 665, 664, -1, 664, 666, 88, -1, 667, 666, 668, -1, 668, 666, 654, -1, 654, 666, 664, -1, 666, 77, 88, -1, 77, 666, 667, -1, 78, 77, 667, -1, 78, 667, 669, -1, 669, 670, 78, -1, 79, 78, 670, -1, 79, 670, 532, -1, 532, 80, 79, -1, 531, 438, 642, -1, 642, 662, 531, -1, 643, 671, 642, -1, 644, 671, 643, -1, 641, 671, 644, -1, 662, 671, 641, -1, 642, 671, 662, -1, 667, 668, 672, -1, 669, 667, 672, -1, 672, 673, 669, -1, 670, 669, 673, -1, 673, 534, 532, -1, 532, 670, 673, -1, 654, 674, 668, -1, 672, 668, 674, -1, 673, 672, 674, -1, 674, 675, 673, -1, 675, 535, 534, -1, 534, 673, 675, -1, 676, 440, 535, -1, 654, 676, 674, -1, 674, 676, 675, -1, 676, 535, 675, -1, 440, 676, 654, -1, 68, 677, 97, -1, 678, 677, 68, -1, 31, 678, 68, -1, 679, 677, 678, -1, 97, 677, 679, -1, 9, 97, 679, -1, 536, 537, 680, -1, 538, 536, 680, -1, 539, 538, 680, -1, 540, 539, 680, -1, 541, 540, 680, -1, 542, 541, 680, -1, 543, 542, 680, -1, 544, 543, 680, -1, 545, 544, 680, -1, 546, 545, 680, -1, 547, 546, 680, -1, 548, 547, 680, -1, 549, 548, 680, -1, 550, 549, 680, -1, 551, 550, 680, -1, 552, 551, 680, -1, 553, 552, 680, -1, 554, 553, 680, -1, 555, 554, 680, -1, 555, 680, 556, -1, 556, 680, 557, -1, 557, 680, 558, -1, 558, 680, 559, -1, 559, 680, 560, -1, 560, 680, 561, -1, 561, 680, 562, -1, 562, 680, 563, -1, 563, 680, 564, -1, 564, 680, 565, -1, 565, 680, 566, -1, 566, 680, 567, -1, 567, 680, 568, -1, 680, 569, 568, -1, 680, 570, 569, -1, 680, 571, 570, -1, 680, 572, 571, -1, 680, 573, 572, -1, 680, 574, 573, -1, 680, 575, 574, -1, 680, 576, 575, -1, 680, 577, 576, -1, 680, 578, 577, -1, 680, 579, 578, -1, 680, 580, 579, -1, 680, 581, 580, -1, 680, 582, 581, -1, 583, 582, 680, -1, 584, 583, 680, -1, 585, 584, 680, -1, 586, 585, 680, -1, 587, 586, 680, -1, 537, 587, 680, -1, 588, 681, 589, -1, 590, 681, 588, -1, 591, 681, 590, -1, 592, 681, 591, -1, 593, 681, 592, -1, 594, 681, 593, -1, 595, 681, 594, -1, 595, 596, 681, -1, 596, 597, 681, -1, 597, 598, 681, -1, 598, 599, 681, -1, 599, 600, 681, -1, 600, 601, 681, -1, 601, 602, 681, -1, 602, 603, 681, -1, 603, 604, 681, -1, 604, 605, 681, -1, 605, 606, 681, -1, 606, 607, 681, -1, 607, 608, 681, -1, 608, 609, 681, -1, 609, 610, 681, -1, 610, 611, 681, -1, 611, 612, 681, -1, 612, 613, 681, -1, 613, 614, 681, -1, 614, 615, 681, -1, 615, 616, 681, -1, 616, 617, 681, -1, 617, 618, 681, -1, 618, 619, 681, -1, 619, 620, 681, -1, 681, 620, 621, -1, 681, 621, 622, -1, 681, 622, 623, -1, 681, 623, 624, -1, 681, 624, 625, -1, 681, 625, 626, -1, 681, 626, 627, -1, 681, 627, 628, -1, 681, 628, 629, -1, 681, 629, 630, -1, 681, 630, 631, -1, 681, 631, 632, -1, 681, 632, 633, -1, 634, 681, 633, -1, 635, 681, 634, -1, 636, 681, 635, -1, 637, 681, 636, -1, 638, 681, 637, -1, 639, 681, 638, -1, 589, 681, 639, -1, 74, 92, 72, -1, 92, 514, 72, -1, 92, 658, 58, -1, 58, 658, 57, -1, 58, 662, 641, -1, 682, 56, 683, -1, 682, 683, 58, -1, 682, 58, 56, -1, 683, 662, 58, -1, 58, 641, 514, -1, 514, 92, 58, -1, 646, 645, 42, -1, 646, 52, 69, -1, 69, 52, 28, -1, 646, 42, 52, -1, 68, 27, 31, -1, 68, 28, 27, -1, 33, 683, 56, -1, 56, 684, 33, -1, 685, 33, 42, -1, 26, 685, 42, -1, 66, 683, 685, -1, 26, 66, 685, -1, 685, 683, 33, -1, 686, 64, 66, -1, 66, 687, 688, -1, 688, 686, 66, -1, 66, 689, 687, -1, 686, 690, 64, -1, 691, 664, 665, -1, 66, 692, 689, -1, 693, 664, 691, -1, 693, 654, 664, -1, 23, 654, 694, -1, 694, 654, 695, -1, 695, 654, 696, -1, 696, 654, 693, -1, 649, 652, 26, -1, 64, 690, 697, -1, 652, 653, 22, -1, 653, 654, 22, -1, 652, 22, 26, -1, 22, 654, 23, -1, 66, 26, 698, -1, 66, 698, 692, -1, 697, 663, 64, -1, 26, 699, 698, -1, 26, 25, 699, -1, 700, 98, 20, -1, 20, 701, 700, -1, 702, 701, 20, -1, 19, 702, 20, -1, 702, 703, 701, -1, 20, 655, 10, -1, 655, 656, 10, -1, 656, 5, 10, -1, 10, 5, 11, -1, 704, 705, 706, -1, 707, 705, 704, -1, 707, 5, 705, -1, 708, 5, 709, -1, 11, 5, 708, -1, 709, 5, 710, -1, 710, 5, 711, -1, 711, 5, 707, -1, 701, 703, 712, -1, 712, 713, 701, -1, 6, 97, 9, -1, 97, 6, 5, -1, 93, 98, 700, -1, 714, 93, 700, -1, 714, 659, 93, -1, 659, 714, 57, -1, 715, 660, 659, -1, 715, 661, 660, -1, 715, 658, 661, -1, 715, 57, 658, -1, 715, 659, 57, -1, 662, 683, 66, -1, 716, 530, 663, -1, 716, 663, 717, -1, 718, 716, 717, -1, 717, 719, 718, -1, 719, 720, 718, -1, 720, 719, 721, -1, 722, 720, 721, -1, 721, 723, 722, -1, 724, 722, 723, -1, 724, 723, 725, -1, 726, 724, 725, -1, 725, 727, 726, -1, 728, 726, 727, -1, 727, 729, 728, -1, 729, 730, 728, -1, 730, 731, 728, -1, 530, 716, 62, -1, 716, 718, 62, -1, 718, 720, 60, -1, 62, 718, 60, -1, 720, 722, 60, -1, 60, 722, 724, -1, 60, 724, 726, -1, 60, 726, 728, -1, 60, 728, 731, -1, 60, 731, 732, -1, 60, 732, 733, -1, 60, 733, 734, -1, 61, 60, 735, -1, 60, 734, 735, -1, 732, 731, 730, -1, 732, 730, 736, -1, 733, 732, 736, -1, 733, 736, 737, -1, 737, 734, 733, -1, 737, 738, 734, -1, 735, 734, 738, -1, 735, 738, 739, -1, 61, 739, 665, -1, 61, 735, 739, -1, 29, 28, 740, -1, 29, 740, 741, -1, 741, 742, 29, -1, 30, 29, 742, -1, 30, 742, 678, -1, 678, 31, 30, -1, 741, 740, 743, -1, 740, 744, 743, -1, 743, 745, 741, -1, 742, 741, 745, -1, 745, 679, 678, -1, 678, 742, 745, -1, 5, 7, 744, -1, 743, 744, 7, -1, 7, 8, 745, -1, 745, 743, 7, -1, 8, 9, 679, -1, 679, 745, 8, -1, 57, 714, 55, -1, 700, 55, 714, -1, 37, 33, 684, -1, 701, 37, 684, -1, 701, 684, 55, -1, 701, 55, 700, -1, 55, 684, 56, -1, 713, 37, 701, -1, 43, 37, 713, -1, 50, 0, 4, -1, 49, 0, 50, -1, 48, 0, 49, -1, 47, 0, 48, -1, 46, 0, 47, -1, 53, 0, 46, -1, 54, 0, 53, -1, 54, 45, 0, -1, 45, 44, 0, -1, 44, 41, 0, -1, 41, 39, 0, -1, 39, 36, 0, -1, 36, 32, 0, -1, 32, 34, 0, -1, 34, 3, 0, -1, 34, 35, 3, -1, 35, 38, 3, -1, 38, 40, 3, -1, 40, 43, 3, -1, 2, 706, 705, -1, 51, 2, 705, -1, 740, 746, 744, -1, 744, 746, 5, -1, 5, 746, 705, -1, 705, 746, 51, -1, 746, 28, 51, -1, 28, 746, 740, -1, 691, 665, 747, -1, 747, 665, 748, -1, 749, 747, 748, -1, 693, 691, 747, -1, 696, 693, 747, -1, 695, 696, 747, -1, 694, 695, 747, -1, 23, 694, 747, -1, 21, 23, 747, -1, 24, 21, 747, -1, 25, 24, 747, -1, 699, 25, 747, -1, 698, 699, 747, -1, 692, 698, 747, -1, 689, 692, 747, -1, 687, 689, 747, -1, 687, 747, 688, -1, 688, 747, 749, -1, 686, 688, 749, -1, 686, 749, 690, -1, 690, 749, 697, -1, 697, 749, 663, -1, 704, 706, 750, -1, 750, 706, 751, -1, 752, 750, 751, -1, 707, 704, 750, -1, 711, 707, 750, -1, 710, 711, 750, -1, 709, 710, 750, -1, 708, 709, 750, -1, 11, 708, 750, -1, 12, 11, 750, -1, 13, 12, 750, -1, 14, 13, 750, -1, 15, 14, 750, -1, 16, 15, 750, -1, 17, 16, 750, -1, 18, 17, 750, -1, 18, 750, 19, -1, 19, 750, 752, -1, 702, 19, 752, -1, 702, 752, 703, -1, 703, 752, 712, -1, 712, 752, 713, -1, 663, 749, 717, -1, 717, 749, 719, -1, 719, 748, 721, -1, 749, 748, 719, -1, 721, 748, 723, -1, 748, 725, 723, -1, 748, 727, 725, -1, 748, 729, 727, -1, 748, 730, 729, -1, 736, 730, 748, -1, 737, 736, 748, -1, 738, 737, 748, -1, 665, 739, 748, -1, 739, 738, 748, -1, 753, 43, 713, -1, 753, 713, 754, -1, 755, 753, 754, -1, 754, 756, 755, -1, 756, 757, 755, -1, 757, 756, 758, -1, 759, 757, 758, -1, 758, 760, 759, -1, 761, 759, 760, -1, 761, 760, 762, -1, 763, 761, 762, -1, 762, 764, 763, -1, 765, 763, 764, -1, 764, 766, 765, -1, 766, 767, 765, -1, 767, 768, 765, -1, 43, 753, 3, -1, 753, 755, 3, -1, 755, 757, 1, -1, 3, 755, 1, -1, 757, 759, 1, -1, 1, 759, 761, -1, 1, 761, 763, -1, 1, 763, 765, -1, 1, 765, 768, -1, 1, 768, 769, -1, 1, 769, 770, -1, 1, 770, 771, -1, 2, 1, 772, -1, 1, 771, 772, -1, 769, 768, 767, -1, 769, 767, 773, -1, 770, 769, 773, -1, 770, 773, 774, -1, 774, 771, 770, -1, 774, 775, 771, -1, 772, 771, 775, -1, 772, 775, 776, -1, 2, 776, 706, -1, 2, 772, 776, -1, 713, 752, 754, -1, 754, 752, 756, -1, 756, 751, 758, -1, 752, 751, 756, -1, 758, 751, 760, -1, 751, 762, 760, -1, 751, 764, 762, -1, 751, 766, 764, -1, 751, 767, 766, -1, 773, 767, 751, -1, 774, 773, 751, -1, 775, 774, 751, -1, 706, 776, 751, -1, 776, 775, 751, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 10.500 -0.500 64.702 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 10.500 -0.500 -123.702 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 10.500 -94.702 -29.500 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 10.500 93.702 -29.500 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -83.702 -0.500 -29.500 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 104.702 -0.500 -29.500 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_HIP_P.wrl000066400000000000000000001514211207742442300227410ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 45.489 37.750 69.314 center 0.000 0.000 0.000 #translation -13.611 -3.975 25.662 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 36.355 14.100 -43.837, 36.355 -10.900 -43.837, 1.009 14.100 -60.319, 1.009 -10.900 -60.319, -9.134 -10.900 -38.567, -9.134 14.100 -38.567, 3.643 14.100 -49.705, 4.055 14.100 -49.882, 4.485 14.100 -50.011, 3.253 14.100 -49.483, 4.926 14.100 -50.092, 5.373 14.100 -50.123, 2.890 14.100 -49.219, 2.560 14.100 -48.916, 5.822 14.100 -50.103, 2.265 14.100 -48.578, 6.265 14.100 -50.034, 2.010 14.100 -48.209, 6.697 14.100 -49.915, 1.798 14.100 -47.814, 7.114 14.100 -49.748, 7.509 14.100 -49.536, 7.878 14.100 -49.281, 8.216 14.100 -48.987, 1.632 14.100 -47.397, 1.513 14.100 -46.965, 1.443 14.100 -46.521, 1.424 14.100 -46.073, 1.454 14.100 -45.626, 1.535 14.100 -45.184, 1.664 14.100 -44.755, 1.841 14.100 -44.343, 2.063 14.100 -43.953, 2.103 14.100 -36.637, 4.149 14.100 -42.331, 3.733 14.100 -42.498, 4.582 14.100 -42.213, 5.025 14.100 -42.143, 5.473 14.100 -42.123, 5.921 14.100 -42.154, 6.362 14.100 -42.235, 6.791 14.100 -42.364, 2.327 14.100 -43.590, 2.630 14.100 -43.260, 2.968 14.100 -42.965, 3.337 14.100 -42.710, 0.835 14.100 -33.919, 9.005 14.100 -47.903, 9.182 14.100 -47.491, 8.783 14.100 -48.293, 8.519 14.100 -48.656, 13.611 14.100 -41.202, 7.204 14.100 -42.541, 7.593 14.100 -42.763, 7.956 14.100 -43.027, 8.287 14.100 -43.330, 8.581 14.100 -43.668, 8.836 14.100 -44.037, 9.048 14.100 -44.433, 9.215 14.100 -44.849, 9.334 14.100 -45.282, 9.403 14.100 -45.725, 9.423 14.100 -46.173, 9.392 14.100 -46.620, 9.312 14.100 -47.062, 27.480 14.100 -24.804, 31.284 1.600 -32.961, 27.480 -10.900 -24.804, 26.212 -10.900 -22.085, 26.212 14.100 -22.085, 2.379 -10.900 -53.521, 3.211 -10.900 -53.811, 1.583 -10.900 -53.141, 4.070 -10.900 -54.008, 0.835 -10.900 -52.676, 4.946 -10.900 -54.109, 0.142 -10.900 -52.132, 5.827 -10.900 -54.113, -0.487 -10.900 -51.514, 6.704 -10.900 -54.020, -1.045 -10.900 -50.831, 7.565 -10.900 -53.831, -1.523 -10.900 -50.091, 8.400 -10.900 -53.549, -1.918 -10.900 -49.303, 9.198 -10.900 -53.176, 9.951 -10.900 -52.718, -2.223 -10.900 -48.476, -2.435 -10.900 -47.621, -2.552 -10.900 -46.747, -2.573 -10.900 -45.866, -2.496 -10.900 -44.988, -2.323 -10.900 -44.124, -2.056 -10.900 -43.284, -1.698 -10.900 -42.478, -1.254 -10.900 -41.717, -0.729 -10.900 -41.009, -0.129 -10.900 -40.363, 2.042 -10.900 -38.873, 2.860 -10.900 -38.545, 2.103 -10.900 -36.637, 3.710 -10.900 -38.309, 4.580 -10.900 -38.168, 5.460 -10.900 -38.123, 0.539 -10.900 -39.787, 1.265 -10.900 -39.289, 0.836 -10.900 -33.919, 13.352 -10.900 -45.061, 13.421 -10.900 -45.939, 13.393 -10.900 -46.820, 13.268 -10.900 -47.693, 13.047 -10.900 -48.546, 12.735 -10.900 -49.370, 12.333 -10.900 -50.155, 11.848 -10.900 -50.891, 11.284 -10.900 -51.568, 10.650 -10.900 -52.180, 8.056 -10.900 -38.569, 8.871 -10.900 -38.904, 9.644 -10.900 -39.327, 6.340 -10.900 -38.176, 7.209 -10.900 -38.325, 10.366 -10.900 -39.833, 13.611 -10.900 -41.202, 11.028 -10.900 -40.415, 11.622 -10.900 -41.066, 12.141 -10.900 -41.778, 12.578 -10.900 -42.544, 12.928 -10.900 -43.352, 13.187 -10.900 -44.195, 8.539 1.600 -30.326, 3.733 15.100 -42.498, 4.149 15.100 -42.331, 4.582 15.100 -42.213, 5.025 15.100 -42.143, 5.473 15.100 -42.123, 5.921 15.100 -42.154, 6.362 15.100 -42.235, 6.791 15.100 -42.364, 7.204 15.100 -42.541, 7.593 15.100 -42.763, 7.956 15.100 -43.027, 8.287 15.100 -43.330, 8.581 15.100 -43.668, 8.836 15.100 -44.037, 9.048 15.100 -44.433, 9.215 15.100 -44.849, 9.334 15.100 -45.282, 9.403 15.100 -45.725, 9.423 15.100 -46.173, 9.392 15.100 -46.620, 9.312 15.100 -47.062, 9.182 15.100 -47.491, 9.005 15.100 -47.903, 8.783 15.100 -48.293, 8.519 15.100 -48.656, 8.216 15.100 -48.987, 7.878 15.100 -49.281, 7.509 15.100 -49.536, 7.114 15.100 -49.748, 6.697 15.100 -49.915, 6.265 15.100 -50.034, 5.822 15.100 -50.103, 5.373 15.100 -50.123, 4.926 15.100 -50.092, 4.485 15.100 -50.011, 4.055 15.100 -49.882, 3.643 15.100 -49.705, 3.253 15.100 -49.483, 2.890 15.100 -49.219, 2.560 15.100 -48.916, 2.265 15.100 -48.578, 2.010 15.100 -48.209, 1.798 15.100 -47.814, 1.632 15.100 -47.397, 1.513 15.100 -46.965, 1.443 15.100 -46.521, 1.424 15.100 -46.073, 1.454 15.100 -45.626, 1.535 15.100 -45.184, 1.664 15.100 -44.755, 1.841 15.100 -44.343, 2.063 15.100 -43.953, 2.327 15.100 -43.590, 2.630 15.100 -43.260, 2.968 15.100 -42.965, 3.337 15.100 -42.710, 2.103 16.100 -36.637, 27.480 16.100 -24.804, -1.705 16.100 -28.471, 0.199 15.100 -32.554, -1.705 14.100 -28.471, 26.212 -12.900 -22.085, 27.480 -12.900 -24.804, 25.367 15.100 -20.273, 23.254 14.100 -15.741, 23.254 16.100 -15.741, 2.042 -11.900 -38.873, 2.860 -11.900 -38.545, 3.710 -11.900 -38.309, 4.580 -11.900 -38.168, 5.460 -11.900 -38.123, 6.340 -11.900 -38.176, 7.209 -11.900 -38.325, 8.056 -11.900 -38.569, 8.871 -11.900 -38.904, 9.644 -11.900 -39.327, 10.366 -11.900 -39.833, 11.028 -11.900 -40.415, 11.622 -11.900 -41.066, 12.141 -11.900 -41.778, 12.578 -11.900 -42.544, 12.928 -11.900 -43.352, 13.187 -11.900 -44.195, 13.352 -11.900 -45.061, 13.421 -11.900 -45.939, 13.393 -11.900 -46.820, 13.268 -11.900 -47.693, 13.047 -11.900 -48.546, 12.735 -11.900 -49.370, 12.333 -11.900 -50.155, 11.848 -11.900 -50.891, 11.284 -11.900 -51.568, 10.650 -11.900 -52.180, 9.951 -11.900 -52.718, 9.198 -11.900 -53.176, 8.400 -11.900 -53.549, 7.565 -11.900 -53.831, 6.704 -11.900 -54.020, 5.827 -11.900 -54.113, 4.946 -11.900 -54.109, 4.070 -11.900 -54.008, 3.211 -11.900 -53.811, 2.379 -11.900 -53.521, 1.583 -11.900 -53.141, 0.835 -11.900 -52.676, 0.142 -11.900 -52.132, -0.487 -11.900 -51.514, -1.045 -11.900 -50.831, -1.523 -11.900 -50.091, -1.918 -11.900 -49.303, -2.223 -11.900 -48.476, -2.435 -11.900 -47.621, -2.552 -11.900 -46.747, -2.573 -11.900 -45.866, -2.496 -11.900 -44.988, -2.323 -11.900 -44.124, -2.056 -11.900 -43.284, -1.698 -11.900 -42.478, -1.254 -11.900 -41.717, -0.729 -11.900 -41.009, -0.129 -11.900 -40.363, 0.539 -11.900 -39.787, 1.265 -11.900 -39.289, 2.103 -12.900 -36.637, 0.836 -12.900 -33.919, 6.997 14.100 -2.878, 7.518 14.100 -1.736, 7.853 14.100 -0.526, -2.736 14.100 -6.518, -1.526 14.100 -6.853, 7.995 14.100 0.721, -3.878 14.100 -5.997, 7.940 14.100 1.975, -4.925 14.100 -5.304, 7.250 14.100 4.381, 7.690 14.100 3.205, 7.457 14.100 4.477, 6.762 14.100 -13.501, 6.304 14.100 -3.925, 5.456 14.100 -4.851, 4.474 14.100 -5.632, -5.851 14.100 -4.456, 3.381 14.100 -6.250, 2.205 14.100 -6.690, 0.975 14.100 -6.940, -0.279 14.100 -6.995, -6.632 14.100 -3.474, -7.250 14.100 -2.381, 5.423 15.100 -46.123, 3.733 15.100 -42.498, 4.149 15.100 -42.331, 4.582 15.100 -42.213, 5.025 15.100 -42.143, 5.473 15.100 -42.123, 5.921 15.100 -42.154, 6.362 15.100 -42.235, 6.791 15.100 -42.364, 7.204 15.100 -42.541, 7.593 15.100 -42.763, 7.956 15.100 -43.027, 8.287 15.100 -43.330, 8.581 15.100 -43.668, 8.836 15.100 -44.037, 9.048 15.100 -44.433, 9.215 15.100 -44.849, 9.334 15.100 -45.282, 9.403 15.100 -45.725, 9.423 15.100 -46.173, 9.392 15.100 -46.620, 9.312 15.100 -47.062, 9.182 15.100 -47.491, 9.005 15.100 -47.903, 8.783 15.100 -48.293, 8.519 15.100 -48.656, 8.216 15.100 -48.987, 7.878 15.100 -49.281, 7.509 15.100 -49.536, 7.114 15.100 -49.748, 6.697 15.100 -49.915, 6.265 15.100 -50.034, 5.822 15.100 -50.103, 5.373 15.100 -50.123, 4.926 15.100 -50.092, 4.485 15.100 -50.011, 4.055 15.100 -49.882, 3.643 15.100 -49.705, 2.890 15.100 -49.219, 2.560 15.100 -48.916, 2.265 15.100 -48.578, 2.010 15.100 -48.209, 1.798 15.100 -47.814, 1.632 15.100 -47.397, 1.513 15.100 -46.965, 1.443 15.100 -46.521, 1.424 15.100 -46.073, 1.454 15.100 -45.626, 1.535 15.100 -45.184, 1.664 15.100 -44.755, 1.841 15.100 -44.343, 2.063 15.100 -43.953, 2.327 15.100 -43.590, 2.630 15.100 -43.260, 2.968 15.100 -42.965, 3.337 15.100 -42.710, 7.727 16.100 -1.072, 6.821 16.100 -4.513, 7.040 16.100 -2.799, 7.997 16.100 0.766, -1.705 16.100 -28.471, 7.000 16.100 -34.000, 2.103 16.100 -36.637, 7.835 16.100 2.617, 7.457 16.100 4.477, 7.250 16.100 4.381, 27.480 16.100 -24.804, 18.148 16.100 -28.802, -1.615 16.100 -6.835, -4.326 16.100 -9.711, -3.379 16.100 -6.251, -4.961 16.100 -5.276, 0.236 16.100 -6.997, -6.275 16.100 -3.962, 2.074 16.100 -6.726, -7.250 16.100 -2.381, 4.586 16.100 -5.555, 5.974 16.100 -4.321, -7.250 14.100 -2.381, -1.705 14.100 -28.471, 27.480 -14.900 -24.804, 25.367 -13.900 -20.273, 23.254 -14.900 -15.741, 23.254 -12.900 -15.741, 7.457 14.100 4.477, 1.583 -11.900 -53.141, 2.379 -11.900 -53.521, 5.025 -11.900 -47.040, 5.139 -11.900 -47.082, 3.211 -11.900 -53.811, 4.917 -11.900 -46.986, 0.835 -11.900 -52.676, 5.256 -11.900 -47.109, 4.070 -11.900 -54.008, 4.946 -11.900 -54.109, 5.376 -11.900 -47.122, 5.827 -11.900 -54.113, 4.817 -11.900 -46.919, 0.142 -11.900 -52.132, 4.726 -11.900 -46.840, -0.487 -11.900 -51.514, 5.497 -11.900 -47.120, 4.644 -11.900 -46.750, -1.045 -11.900 -50.831, 5.617 -11.900 -47.104, 7.565 -11.900 -53.831, 4.574 -11.900 -46.652, -1.523 -11.900 -50.091, 8.400 -11.900 -53.549, 5.734 -11.900 -47.074, 4.517 -11.900 -46.546, -1.918 -11.900 -49.303, 9.198 -11.900 -53.176, 5.846 -11.900 -47.029, 9.951 -11.900 -52.718, 5.952 -11.900 -46.972, 4.473 -11.900 -46.433, -2.223 -11.900 -48.476, 4.442 -11.900 -46.317, -2.435 -11.900 -47.621, 10.650 -11.900 -52.180, 6.050 -11.900 -46.902, 4.426 -11.900 -46.197, -2.552 -11.900 -46.747, -2.573 -11.900 -45.866, 6.140 -11.900 -46.821, 11.284 -11.900 -51.568, 6.219 -11.900 -46.729, 11.848 -11.900 -50.891, 4.424 -11.900 -46.076, -2.496 -11.900 -44.988, -2.323 -11.900 -44.124, 4.437 -11.900 -45.956, 6.286 -11.900 -46.629, 12.333 -11.900 -50.155, 12.735 -11.900 -49.370, 4.465 -11.900 -45.838, 6.341 -11.900 -46.521, -2.056 -11.900 -43.284, 4.506 -11.900 -45.725, 13.047 -11.900 -48.546, 6.382 -11.900 -46.408, -1.698 -11.900 -42.478, 4.561 -11.900 -45.617, 6.409 -11.900 -46.290, 13.268 -11.900 -47.693, -1.254 -11.900 -41.717, 4.628 -11.900 -45.517, 13.393 -11.900 -46.820, 6.422 -11.900 -46.170, -0.729 -11.900 -41.009, 4.707 -11.900 -45.426, 6.421 -11.900 -46.049, 13.421 -11.900 -45.939, -0.129 -11.900 -40.363, 4.796 -11.900 -45.344, 6.404 -11.900 -45.930, 13.352 -11.900 -45.061, 4.894 -11.900 -45.274, 0.539 -11.900 -39.787, 6.374 -11.900 -45.813, 13.187 -11.900 -44.195, 5.001 -11.900 -45.217, 1.265 -11.900 -39.289, 6.330 -11.900 -45.701, 12.928 -11.900 -43.352, 2.042 -11.900 -38.873, 6.272 -11.900 -45.594, 12.578 -11.900 -42.544, 5.113 -11.900 -45.172, 2.860 -11.900 -38.545, 6.202 -11.900 -45.496, 12.141 -11.900 -41.778, 5.230 -11.900 -45.142, 3.710 -11.900 -38.309, 11.622 -11.900 -41.066, 6.121 -11.900 -45.407, 5.349 -11.900 -45.126, 4.580 -11.900 -38.168, 11.028 -11.900 -40.415, 5.470 -11.900 -45.124, 5.460 -11.900 -38.123, 10.366 -11.900 -39.833, 6.029 -11.900 -45.328, 6.340 -11.900 -38.176, 5.590 -11.900 -45.137, 5.929 -11.900 -45.260, 9.644 -11.900 -39.327, 5.708 -11.900 -45.164, 7.209 -11.900 -38.325, 5.821 -11.900 -45.206, 8.871 -11.900 -38.904, 8.056 -11.900 -38.569, 2.103 -14.900 -36.637, -1.705 -12.900 -28.471, 0.199 -13.900 -32.554, -1.705 -14.900 -28.471, 7.250 14.100 4.381, -2.294 14.100 -2.277, -3.064 14.100 -1.571, 0.695 14.100 -2.939, -0.349 14.100 -2.985, -3.625 14.100 -0.690, 1.690 14.100 -2.625, 2.571 14.100 -2.064, 3.277 14.100 -1.294, 3.759 14.100 -0.368, 3.985 14.100 0.651, -1.368 14.100 -2.759, 3.939 14.100 1.695, 3.625 14.100 2.690, 7.518 -12.900 -1.736, 6.997 -12.900 -2.878, 7.853 -12.900 -0.526, 7.995 -12.900 0.721, -1.526 -12.900 -6.853, -2.736 -12.900 -6.518, 7.940 -12.900 1.975, -3.878 -12.900 -5.997, 7.457 -12.900 4.477, 7.690 -12.900 3.205, 7.250 -12.900 4.381, -4.925 -12.900 -5.304, 6.762 -12.900 -13.501, 6.304 -12.900 -3.925, 5.456 -12.900 -4.851, 4.474 -12.900 -5.632, 3.381 -12.900 -6.250, 2.205 -12.900 -6.690, 0.975 -12.900 -6.940, -5.851 -12.900 -4.456, -0.279 -12.900 -6.995, -6.632 -12.900 -3.474, -7.250 -12.900 -2.381, 6.821 22.850 -4.513, 1.248 19.475 -7.112, -4.326 22.850 -9.711, 3.277 16.100 -1.294, 2.571 16.100 -2.064, 3.759 16.100 -0.368, 3.985 16.100 0.651, 3.939 16.100 1.695, 3.625 16.100 2.690, -1.368 16.100 -2.759, -0.349 16.100 -2.985, -2.294 16.100 -2.277, 0.695 16.100 -2.939, -3.064 16.100 -1.571, 1.690 16.100 -2.625, -3.625 16.100 -0.690, 18.148 22.850 -28.802, 7.000 22.850 -34.000, -7.690 16.100 -1.205, -7.690 14.100 -1.205, -7.940 16.100 0.025, -7.940 14.100 0.025, -7.995 16.100 1.279, -7.995 14.100 1.279, -7.853 16.100 2.526, -7.853 14.100 2.526, -7.518 16.100 3.736, -7.518 14.100 3.736, -6.997 16.100 4.878, -6.997 14.100 4.878, -6.304 16.100 5.925, -6.304 14.100 5.925, -5.456 16.100 6.851, -5.456 14.100 6.851, -4.474 16.100 7.632, -4.474 14.100 7.632, -3.381 16.100 8.250, -3.381 14.100 8.250, 6.997 -14.900 -2.878, 7.518 -14.900 -1.736, 7.853 -14.900 -0.526, 7.995 -14.900 0.721, 7.940 -14.900 1.975, -2.736 -14.900 -6.518, -1.526 -14.900 -6.853, -3.878 -14.900 -5.997, 7.250 -14.900 4.381, 7.690 -14.900 3.205, 7.457 -14.900 4.477, 7.396 -14.900 -14.860, -4.925 -14.900 -5.304, 6.304 -14.900 -3.925, 5.456 -14.900 -4.851, 4.474 -14.900 -5.632, 3.381 -14.900 -6.250, 2.205 -14.900 -6.690, 0.975 -14.900 -6.940, -0.279 -14.900 -6.995, -5.851 -14.900 -4.456, -6.632 -14.900 -3.474, -7.250 -14.900 -2.381, 7.457 -14.900 4.477, 23.254 -14.900 -15.741, 23.254 -12.900 -15.741, 7.457 -12.900 4.477, 5.423 -11.900 -46.123, -1.705 -12.900 -28.471, -7.250 -14.900 -2.381, -7.250 -12.900 -2.381, -1.705 -14.900 -28.471, -2.205 16.100 8.690, -2.205 14.100 8.690, -0.975 16.100 8.940, -0.975 14.100 8.940, 0.279 16.100 8.995, 0.279 14.100 8.995, 1.526 16.100 8.853, 1.526 14.100 8.853, 2.736 16.100 8.518, 2.736 14.100 8.518, 3.878 16.100 7.997, 3.878 14.100 7.997, 4.925 16.100 7.304, 4.925 14.100 7.304, 5.851 16.100 6.456, 5.851 14.100 6.456, 6.632 16.100 5.474, 6.632 14.100 5.474, -3.759 14.100 2.368, -3.985 14.100 1.349, -3.759 16.100 2.368, -3.985 16.100 1.349, -3.277 14.100 3.294, -3.277 16.100 3.294, -0.695 14.100 4.939, -1.690 14.100 4.625, -0.695 16.100 4.939, -1.690 16.100 4.625, -2.571 14.100 4.064, -2.571 16.100 4.064, 0.349 14.100 4.985, 0.349 16.100 4.985, 1.368 14.100 4.759, 1.368 16.100 4.759, 2.294 14.100 4.277, 2.294 16.100 4.277, 3.064 14.100 3.571, 3.064 16.100 3.571, -3.939 14.100 0.305, -3.939 16.100 0.305, -4.474 14.100 7.632, -5.456 14.100 6.851, -7.690 14.100 -1.205, -3.381 14.100 8.250, -2.205 14.100 8.690, -7.940 14.100 0.025, -7.995 14.100 1.279, -7.853 14.100 2.526, 6.632 14.100 5.474, -0.975 14.100 8.940, -7.518 14.100 3.736, 5.851 14.100 6.456, -6.997 14.100 4.878, 0.279 14.100 8.995, 4.925 14.100 7.304, -6.304 14.100 5.925, 3.878 14.100 7.997, 1.526 14.100 8.853, 2.736 14.100 8.518, 7.250 -12.900 4.381, 7.250 -14.900 4.381, -3.064 -12.900 -1.571, -2.294 -12.900 -2.277, 0.695 -12.900 -2.939, -0.349 -12.900 -2.985, -3.625 -12.900 -0.690, 1.690 -12.900 -2.625, 2.571 -12.900 -2.064, 3.277 -12.900 -1.294, 3.759 -12.900 -0.368, 3.985 -12.900 0.651, -1.368 -12.900 -2.759, 3.939 -12.900 1.695, 3.625 -12.900 2.690, -4.326 22.850 -9.711, -4.474 16.100 7.632, -5.456 16.100 6.851, -7.250 16.100 -2.381, -7.690 16.100 -1.205, -3.381 16.100 8.250, -2.205 16.100 8.690, -7.940 16.100 0.025, -7.995 16.100 1.279, -7.853 16.100 2.526, 6.632 16.100 5.474, 7.250 16.100 4.381, -0.975 16.100 8.940, -7.518 16.100 3.736, 5.851 16.100 6.456, -6.997 16.100 4.878, 0.279 16.100 8.995, 4.925 16.100 7.304, -6.304 16.100 5.925, 3.878 16.100 7.997, 1.526 16.100 8.853, 2.736 16.100 8.518, -2.294 -14.900 -2.277, -3.064 -14.900 -1.571, 0.695 -14.900 -2.939, -0.349 -14.900 -2.985, -3.625 -14.900 -0.690, 1.690 -14.900 -2.625, 2.571 -14.900 -2.064, 3.277 -14.900 -1.294, 3.759 -14.900 -0.368, 3.985 -14.900 0.651, -1.368 -14.900 -2.759, 3.939 -14.900 1.695, 3.625 -14.900 2.690, -7.690 -12.900 -1.205, -7.690 -14.900 -1.205, -7.940 -12.900 0.025, -7.940 -14.900 0.025, -7.995 -12.900 1.279, -7.995 -14.900 1.279, -7.853 -12.900 2.526, -7.853 -14.900 2.526, -7.518 -12.900 3.736, -7.518 -14.900 3.736, -6.997 -12.900 4.878, -6.997 -14.900 4.878, -6.304 -12.900 5.925, -6.304 -14.900 5.925, -5.456 -12.900 6.851, -5.456 -14.900 6.851, -4.474 -12.900 7.632, -4.474 -14.900 7.632, -3.381 -12.900 8.250, -3.381 -14.900 8.250, -2.205 -12.900 8.690, -2.205 -14.900 8.690, -0.975 -12.900 8.940, -0.975 -14.900 8.940, 0.279 -12.900 8.995, 0.279 -14.900 8.995, 1.526 -12.900 8.853, 1.526 -14.900 8.853, 2.736 -12.900 8.518, 2.736 -14.900 8.518, 3.878 -12.900 7.997, 3.878 -14.900 7.997, 4.925 -12.900 7.304, 4.925 -14.900 7.304, 5.851 -12.900 6.456, 5.851 -14.900 6.456, 6.632 -12.900 5.474, 6.632 -14.900 5.474, -3.759 -14.900 2.368, -3.985 -14.900 1.349, -3.759 -12.900 2.368, -3.985 -12.900 1.349, -3.277 -14.900 3.294, -3.277 -12.900 3.294, -0.695 -14.900 4.939, -1.690 -14.900 4.625, -0.695 -12.900 4.939, -1.690 -12.900 4.625, -2.571 -14.900 4.064, -2.571 -12.900 4.064, 0.349 -14.900 4.985, 0.349 -12.900 4.985, 1.368 -14.900 4.759, 1.368 -12.900 4.759, 2.294 -14.900 4.277, 2.294 -12.900 4.277, 3.064 -14.900 3.571, 3.064 -12.900 3.571, -3.939 -14.900 0.305, -3.939 -12.900 0.305, -4.474 -12.900 7.632, -5.456 -12.900 6.851, -7.690 -12.900 -1.205, -3.381 -12.900 8.250, -2.205 -12.900 8.690, -7.995 -12.900 1.279, 6.632 -12.900 5.474, -0.975 -12.900 8.940, -7.518 -12.900 3.736, 5.851 -12.900 6.456, -6.997 -12.900 4.878, 0.279 -12.900 8.995, 4.925 -12.900 7.304, -6.304 -12.900 5.925, 3.878 -12.900 7.997, 1.526 -12.900 8.853, 2.736 -12.900 8.518, -4.474 -14.900 7.632, -5.456 -14.900 6.851, -7.690 -14.900 -1.205, -3.381 -14.900 8.250, -2.205 -14.900 8.690, -7.995 -14.900 1.279, 6.632 -14.900 5.474, -0.975 -14.900 8.940, 5.851 -14.900 6.456, 0.279 -14.900 8.995, 4.925 -14.900 7.304, -6.304 -14.900 5.925, 3.878 -14.900 7.997, 1.526 -14.900 8.853, 2.736 -14.900 8.518 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 1, 3, 2, -1, 3, 4, 5, -1, 2, 3, 5, -1, 6, 7, 2, -1, 8, 2, 7, -1, 9, 6, 2, -1, 10, 2, 8, -1, 11, 2, 10, -1, 12, 9, 2, -1, 13, 12, 2, -1, 14, 2, 11, -1, 15, 13, 2, -1, 16, 2, 14, -1, 17, 15, 2, -1, 18, 2, 16, -1, 19, 17, 2, -1, 20, 2, 18, -1, 21, 2, 20, -1, 22, 2, 21, -1, 23, 2, 22, -1, 5, 24, 19, -1, 5, 25, 24, -1, 5, 26, 25, -1, 5, 27, 26, -1, 5, 28, 27, -1, 5, 29, 28, -1, 5, 30, 29, -1, 5, 31, 30, -1, 5, 32, 31, -1, 5, 19, 2, -1, 33, 32, 5, -1, 33, 34, 35, -1, 33, 36, 34, -1, 33, 37, 36, -1, 33, 38, 37, -1, 33, 39, 38, -1, 33, 40, 39, -1, 33, 41, 40, -1, 33, 42, 32, -1, 33, 43, 42, -1, 33, 44, 43, -1, 33, 45, 44, -1, 33, 35, 45, -1, 46, 33, 5, -1, 0, 2, 23, -1, 0, 47, 48, -1, 0, 49, 47, -1, 0, 50, 49, -1, 0, 23, 50, -1, 51, 52, 41, -1, 51, 53, 52, -1, 51, 54, 53, -1, 51, 55, 54, -1, 51, 56, 55, -1, 51, 57, 56, -1, 51, 58, 57, -1, 51, 59, 58, -1, 51, 60, 59, -1, 51, 61, 60, -1, 51, 62, 61, -1, 51, 63, 62, -1, 51, 64, 63, -1, 51, 48, 64, -1, 51, 41, 33, -1, 51, 65, 0, -1, 51, 33, 65, -1, 51, 0, 48, -1, 66, 67, 1, -1, 66, 1, 0, -1, 66, 68, 67, -1, 66, 0, 65, -1, 66, 69, 68, -1, 66, 65, 69, -1, 70, 3, 71, -1, 3, 70, 72, -1, 71, 3, 73, -1, 3, 72, 74, -1, 73, 3, 75, -1, 3, 74, 76, -1, 75, 3, 77, -1, 3, 76, 78, -1, 77, 3, 79, -1, 3, 78, 80, -1, 79, 3, 81, -1, 3, 80, 82, -1, 81, 3, 83, -1, 3, 82, 84, -1, 83, 3, 85, -1, 85, 3, 86, -1, 84, 87, 4, -1, 87, 88, 4, -1, 88, 89, 4, -1, 89, 90, 4, -1, 90, 91, 4, -1, 91, 92, 4, -1, 92, 93, 4, -1, 93, 94, 4, -1, 94, 95, 4, -1, 3, 84, 4, -1, 4, 95, 96, -1, 4, 96, 97, -1, 98, 99, 100, -1, 99, 101, 100, -1, 101, 102, 100, -1, 102, 103, 100, -1, 104, 105, 100, -1, 105, 98, 100, -1, 4, 97, 106, -1, 97, 104, 106, -1, 104, 100, 106, -1, 107, 108, 1, -1, 108, 109, 1, -1, 109, 110, 1, -1, 110, 111, 1, -1, 111, 112, 1, -1, 112, 113, 1, -1, 113, 114, 1, -1, 114, 115, 1, -1, 115, 116, 1, -1, 116, 86, 1, -1, 117, 118, 67, -1, 118, 119, 67, -1, 103, 67, 100, -1, 120, 67, 103, -1, 121, 67, 120, -1, 117, 67, 121, -1, 86, 3, 1, -1, 119, 122, 123, -1, 122, 124, 123, -1, 124, 125, 123, -1, 125, 126, 123, -1, 126, 127, 123, -1, 127, 128, 123, -1, 128, 129, 123, -1, 129, 107, 123, -1, 107, 1, 123, -1, 1, 67, 123, -1, 67, 119, 123, -1, 46, 4, 106, -1, 5, 4, 46, -1, 130, 69, 46, -1, 130, 106, 68, -1, 130, 68, 69, -1, 130, 46, 106, -1, 34, 131, 35, -1, 132, 131, 34, -1, 133, 34, 36, -1, 133, 132, 34, -1, 134, 36, 37, -1, 134, 133, 36, -1, 135, 37, 38, -1, 135, 134, 37, -1, 136, 38, 39, -1, 136, 135, 38, -1, 137, 39, 40, -1, 137, 136, 39, -1, 138, 40, 41, -1, 138, 137, 40, -1, 139, 41, 52, -1, 139, 138, 41, -1, 53, 139, 52, -1, 140, 139, 53, -1, 141, 53, 54, -1, 141, 140, 53, -1, 142, 54, 55, -1, 142, 141, 54, -1, 56, 142, 55, -1, 143, 142, 56, -1, 144, 56, 57, -1, 144, 143, 56, -1, 145, 57, 58, -1, 145, 144, 57, -1, 146, 58, 59, -1, 146, 145, 58, -1, 147, 59, 60, -1, 147, 146, 59, -1, 61, 147, 60, -1, 148, 147, 61, -1, 149, 61, 62, -1, 149, 148, 61, -1, 150, 62, 63, -1, 150, 149, 62, -1, 151, 63, 64, -1, 151, 150, 63, -1, 152, 64, 48, -1, 152, 151, 64, -1, 153, 152, 48, -1, 153, 48, 47, -1, 154, 153, 47, -1, 154, 47, 49, -1, 50, 154, 49, -1, 155, 154, 50, -1, 156, 155, 50, -1, 156, 50, 23, -1, 157, 156, 23, -1, 157, 23, 22, -1, 158, 157, 22, -1, 158, 22, 21, -1, 159, 158, 21, -1, 159, 21, 20, -1, 160, 20, 18, -1, 160, 159, 20, -1, 161, 18, 16, -1, 161, 160, 18, -1, 162, 16, 14, -1, 162, 161, 16, -1, 163, 14, 11, -1, 163, 162, 14, -1, 164, 11, 10, -1, 164, 163, 11, -1, 8, 164, 10, -1, 165, 164, 8, -1, 166, 8, 7, -1, 166, 165, 8, -1, 167, 7, 6, -1, 167, 166, 7, -1, 168, 6, 9, -1, 168, 167, 6, -1, 169, 9, 12, -1, 169, 168, 9, -1, 170, 12, 13, -1, 170, 169, 12, -1, 171, 13, 15, -1, 171, 170, 13, -1, 172, 15, 17, -1, 172, 171, 15, -1, 173, 17, 19, -1, 173, 172, 17, -1, 174, 19, 24, -1, 174, 173, 19, -1, 175, 24, 25, -1, 175, 174, 24, -1, 26, 175, 25, -1, 176, 175, 26, -1, 177, 26, 27, -1, 177, 176, 26, -1, 28, 177, 27, -1, 178, 177, 28, -1, 179, 178, 28, -1, 179, 28, 29, -1, 180, 179, 29, -1, 180, 29, 30, -1, 181, 180, 30, -1, 181, 30, 31, -1, 182, 181, 31, -1, 182, 31, 32, -1, 183, 182, 32, -1, 183, 32, 42, -1, 184, 183, 42, -1, 184, 42, 43, -1, 185, 184, 43, -1, 185, 43, 44, -1, 186, 185, 44, -1, 186, 44, 45, -1, 131, 186, 45, -1, 131, 45, 35, -1, 65, 33, 187, -1, 188, 65, 187, -1, 187, 33, 46, -1, 189, 187, 190, -1, 191, 189, 190, -1, 46, 191, 190, -1, 187, 46, 190, -1, 68, 192, 193, -1, 68, 193, 67, -1, 69, 65, 188, -1, 194, 195, 69, -1, 194, 188, 196, -1, 194, 196, 195, -1, 194, 69, 188, -1, 99, 197, 198, -1, 99, 98, 197, -1, 101, 198, 199, -1, 101, 99, 198, -1, 102, 199, 200, -1, 102, 101, 199, -1, 103, 200, 201, -1, 103, 102, 200, -1, 202, 103, 201, -1, 120, 103, 202, -1, 121, 202, 203, -1, 121, 120, 202, -1, 204, 121, 203, -1, 117, 121, 204, -1, 118, 204, 205, -1, 118, 117, 204, -1, 206, 118, 205, -1, 119, 118, 206, -1, 122, 206, 207, -1, 122, 119, 206, -1, 124, 207, 208, -1, 124, 122, 207, -1, 125, 208, 209, -1, 125, 124, 208, -1, 126, 209, 210, -1, 126, 125, 209, -1, 127, 210, 211, -1, 127, 126, 210, -1, 128, 211, 212, -1, 128, 127, 211, -1, 129, 212, 213, -1, 129, 128, 212, -1, 107, 213, 214, -1, 107, 129, 213, -1, 108, 214, 215, -1, 108, 107, 214, -1, 109, 215, 216, -1, 109, 108, 215, -1, 110, 216, 217, -1, 110, 109, 216, -1, 111, 110, 217, -1, 111, 217, 218, -1, 219, 111, 218, -1, 112, 111, 219, -1, 113, 112, 219, -1, 113, 219, 220, -1, 221, 113, 220, -1, 114, 113, 221, -1, 115, 114, 221, -1, 115, 221, 222, -1, 223, 115, 222, -1, 116, 115, 223, -1, 86, 116, 223, -1, 86, 223, 224, -1, 225, 86, 224, -1, 85, 86, 225, -1, 226, 85, 225, -1, 83, 85, 226, -1, 81, 226, 227, -1, 81, 83, 226, -1, 228, 81, 227, -1, 79, 81, 228, -1, 77, 228, 229, -1, 77, 79, 228, -1, 230, 77, 229, -1, 75, 77, 230, -1, 73, 230, 231, -1, 73, 75, 230, -1, 232, 73, 231, -1, 71, 73, 232, -1, 70, 232, 233, -1, 70, 71, 232, -1, 72, 233, 234, -1, 72, 70, 233, -1, 235, 72, 234, -1, 74, 72, 235, -1, 76, 235, 236, -1, 76, 74, 235, -1, 237, 76, 236, -1, 78, 76, 237, -1, 80, 237, 238, -1, 80, 78, 237, -1, 82, 238, 239, -1, 82, 80, 238, -1, 240, 82, 239, -1, 84, 82, 240, -1, 87, 240, 241, -1, 87, 84, 240, -1, 242, 87, 241, -1, 88, 87, 242, -1, 243, 88, 242, -1, 89, 88, 243, -1, 244, 89, 243, -1, 90, 89, 244, -1, 245, 90, 244, -1, 91, 90, 245, -1, 246, 91, 245, -1, 92, 91, 246, -1, 93, 92, 246, -1, 93, 246, 247, -1, 248, 93, 247, -1, 94, 93, 248, -1, 249, 94, 248, -1, 95, 94, 249, -1, 250, 95, 249, -1, 96, 95, 250, -1, 97, 96, 250, -1, 97, 250, 251, -1, 252, 97, 251, -1, 104, 97, 252, -1, 253, 104, 252, -1, 105, 104, 253, -1, 98, 105, 253, -1, 98, 253, 197, -1, 193, 254, 100, -1, 67, 193, 100, -1, 254, 255, 106, -1, 100, 254, 106, -1, 256, 195, 257, -1, 257, 195, 258, -1, 259, 191, 260, -1, 258, 195, 261, -1, 191, 259, 262, -1, 261, 195, 263, -1, 191, 262, 264, -1, 265, 266, 267, -1, 266, 195, 267, -1, 263, 195, 266, -1, 69, 195, 268, -1, 256, 269, 268, -1, 269, 270, 268, -1, 270, 271, 268, -1, 191, 264, 272, -1, 271, 273, 268, -1, 273, 274, 268, -1, 274, 275, 268, -1, 275, 276, 268, -1, 276, 260, 268, -1, 191, 46, 268, -1, 46, 69, 268, -1, 260, 191, 268, -1, 195, 256, 268, -1, 191, 272, 277, -1, 191, 277, 278, -1, 106, 255, 192, -1, 106, 192, 68, -1, 279, 280, 281, -1, 279, 281, 282, -1, 279, 282, 283, -1, 279, 283, 284, -1, 279, 284, 285, -1, 279, 285, 286, -1, 279, 286, 287, -1, 279, 287, 288, -1, 279, 288, 289, -1, 279, 289, 290, -1, 279, 290, 291, -1, 279, 291, 292, -1, 279, 292, 293, -1, 279, 293, 294, -1, 279, 294, 295, -1, 279, 295, 296, -1, 279, 296, 297, -1, 279, 297, 298, -1, 279, 298, 299, -1, 279, 299, 300, -1, 279, 300, 301, -1, 279, 301, 302, -1, 279, 302, 303, -1, 279, 303, 304, -1, 279, 304, 305, -1, 279, 305, 306, -1, 279, 306, 307, -1, 279, 307, 308, -1, 279, 308, 309, -1, 279, 309, 310, -1, 279, 310, 311, -1, 279, 311, 312, -1, 279, 312, 313, -1, 279, 313, 314, -1, 279, 314, 315, -1, 279, 315, 316, -1, 279, 316, 168, -1, 279, 168, 317, -1, 279, 317, 318, -1, 279, 318, 319, -1, 279, 319, 320, -1, 279, 320, 321, -1, 279, 321, 322, -1, 279, 322, 323, -1, 279, 323, 324, -1, 279, 324, 325, -1, 279, 325, 326, -1, 279, 326, 327, -1, 279, 327, 328, -1, 279, 328, 329, -1, 279, 329, 330, -1, 279, 330, 331, -1, 279, 331, 332, -1, 279, 332, 333, -1, 279, 333, 334, -1, 279, 334, 280, -1, 335, 196, 336, -1, 335, 336, 337, -1, 338, 196, 335, -1, 339, 340, 341, -1, 342, 196, 338, -1, 343, 342, 344, -1, 345, 341, 346, -1, 346, 341, 340, -1, 343, 196, 342, -1, 346, 336, 196, -1, 347, 348, 349, -1, 348, 340, 339, -1, 196, 345, 346, -1, 350, 349, 348, -1, 351, 348, 347, -1, 352, 350, 348, -1, 353, 348, 351, -1, 354, 348, 339, -1, 354, 352, 348, -1, 336, 355, 356, -1, 337, 336, 356, -1, 339, 357, 354, -1, 339, 358, 357, -1, 192, 359, 193, -1, 360, 361, 359, -1, 360, 362, 361, -1, 360, 192, 362, -1, 360, 359, 192, -1, 363, 195, 196, -1, 343, 363, 196, -1, 364, 365, 366, -1, 366, 365, 367, -1, 365, 368, 367, -1, 364, 366, 369, -1, 370, 364, 369, -1, 367, 368, 371, -1, 372, 373, 371, -1, 368, 372, 371, -1, 371, 373, 374, -1, 373, 375, 374, -1, 370, 369, 376, -1, 377, 370, 376, -1, 377, 376, 378, -1, 379, 377, 378, -1, 374, 375, 380, -1, 375, 228, 380, -1, 379, 378, 381, -1, 382, 379, 381, -1, 380, 228, 383, -1, 228, 384, 383, -1, 382, 381, 385, -1, 386, 382, 385, -1, 384, 387, 388, -1, 383, 384, 388, -1, 386, 385, 389, -1, 390, 386, 389, -1, 387, 391, 392, -1, 388, 387, 392, -1, 391, 393, 394, -1, 392, 391, 394, -1, 390, 389, 395, -1, 396, 390, 395, -1, 396, 395, 397, -1, 398, 396, 397, -1, 393, 399, 400, -1, 394, 393, 400, -1, 398, 397, 401, -1, 402, 398, 401, -1, 403, 402, 401, -1, 400, 399, 404, -1, 399, 405, 404, -1, 404, 405, 406, -1, 405, 407, 406, -1, 403, 401, 408, -1, 409, 403, 408, -1, 410, 409, 411, -1, 409, 408, 411, -1, 406, 407, 412, -1, 413, 414, 412, -1, 407, 413, 412, -1, 410, 411, 415, -1, 412, 414, 416, -1, 410, 415, 417, -1, 415, 418, 417, -1, 416, 414, 419, -1, 420, 416, 419, -1, 417, 418, 421, -1, 418, 422, 421, -1, 423, 420, 424, -1, 420, 419, 424, -1, 421, 422, 425, -1, 422, 426, 425, -1, 423, 424, 427, -1, 428, 423, 427, -1, 425, 426, 429, -1, 426, 430, 429, -1, 431, 428, 432, -1, 428, 427, 432, -1, 429, 430, 433, -1, 430, 434, 433, -1, 435, 431, 436, -1, 431, 432, 436, -1, 434, 437, 438, -1, 433, 434, 438, -1, 439, 435, 440, -1, 435, 436, 440, -1, 437, 441, 442, -1, 438, 437, 442, -1, 443, 439, 444, -1, 439, 440, 444, -1, 442, 441, 445, -1, 446, 443, 447, -1, 443, 444, 447, -1, 441, 448, 449, -1, 445, 441, 449, -1, 450, 446, 451, -1, 446, 447, 451, -1, 448, 452, 453, -1, 449, 448, 453, -1, 450, 451, 454, -1, 455, 450, 454, -1, 452, 456, 457, -1, 453, 452, 457, -1, 455, 454, 458, -1, 456, 459, 460, -1, 457, 456, 460, -1, 455, 458, 461, -1, 462, 455, 461, -1, 460, 459, 463, -1, 459, 464, 463, -1, 465, 462, 466, -1, 462, 461, 466, -1, 464, 467, 468, -1, 463, 464, 468, -1, 469, 465, 470, -1, 465, 466, 470, -1, 467, 469, 471, -1, 468, 467, 471, -1, 469, 470, 471, -1, 359, 472, 254, -1, 193, 359, 254, -1, 254, 472, 255, -1, 473, 255, 474, -1, 472, 475, 474, -1, 475, 473, 474, -1, 255, 472, 474, -1, 344, 363, 343, -1, 344, 476, 363, -1, 264, 477, 478, -1, 275, 274, 479, -1, 480, 275, 479, -1, 278, 277, 481, -1, 277, 478, 481, -1, 273, 271, 482, -1, 274, 273, 482, -1, 479, 274, 482, -1, 271, 270, 483, -1, 482, 271, 483, -1, 269, 256, 484, -1, 270, 269, 484, -1, 483, 270, 484, -1, 484, 256, 485, -1, 485, 256, 257, -1, 486, 485, 258, -1, 485, 257, 258, -1, 259, 260, 487, -1, 262, 259, 487, -1, 488, 486, 261, -1, 486, 258, 261, -1, 264, 262, 477, -1, 262, 487, 477, -1, 488, 261, 263, -1, 276, 275, 480, -1, 489, 488, 266, -1, 260, 276, 480, -1, 487, 260, 480, -1, 488, 263, 266, -1, 489, 266, 265, -1, 272, 264, 478, -1, 277, 272, 478, -1, 490, 362, 491, -1, 492, 362, 490, -1, 493, 362, 492, -1, 494, 473, 495, -1, 496, 362, 493, -1, 497, 495, 473, -1, 498, 499, 500, -1, 498, 362, 499, -1, 501, 497, 473, -1, 499, 362, 496, -1, 502, 192, 255, -1, 502, 362, 192, -1, 502, 255, 473, -1, 502, 503, 491, -1, 502, 504, 503, -1, 502, 505, 504, -1, 502, 506, 505, -1, 502, 507, 506, -1, 502, 508, 507, -1, 509, 501, 473, -1, 502, 510, 508, -1, 502, 494, 510, -1, 502, 473, 494, -1, 502, 491, 362, -1, 511, 509, 473, -1, 512, 511, 473, -1, 513, 355, 336, -1, 514, 515, 348, -1, 514, 513, 515, -1, 514, 353, 355, -1, 514, 348, 353, -1, 514, 355, 513, -1, 516, 337, 356, -1, 516, 356, 517, -1, 518, 337, 516, -1, 335, 337, 518, -1, 519, 335, 518, -1, 338, 519, 520, -1, 338, 335, 519, -1, 342, 520, 521, -1, 342, 338, 520, -1, 522, 347, 349, -1, 344, 342, 521, -1, 523, 351, 347, -1, 523, 347, 522, -1, 524, 349, 350, -1, 524, 522, 349, -1, 525, 353, 351, -1, 525, 351, 523, -1, 526, 350, 352, -1, 526, 524, 350, -1, 527, 355, 353, -1, 527, 353, 525, -1, 528, 352, 354, -1, 528, 526, 352, -1, 517, 356, 355, -1, 517, 355, 527, -1, 513, 346, 529, -1, 336, 346, 513, -1, 340, 530, 346, -1, 346, 530, 529, -1, 530, 340, 515, -1, 515, 340, 348, -1, 531, 357, 532, -1, 531, 354, 357, -1, 533, 532, 534, -1, 533, 531, 532, -1, 535, 534, 536, -1, 535, 533, 534, -1, 537, 536, 538, -1, 537, 535, 536, -1, 539, 538, 540, -1, 539, 537, 538, -1, 541, 540, 542, -1, 541, 539, 540, -1, 543, 542, 544, -1, 543, 541, 542, -1, 545, 544, 546, -1, 545, 543, 544, -1, 547, 546, 548, -1, 547, 545, 546, -1, 549, 548, 550, -1, 549, 547, 548, -1, 551, 361, 552, -1, 552, 361, 553, -1, 553, 361, 554, -1, 554, 361, 555, -1, 556, 475, 557, -1, 475, 556, 558, -1, 559, 560, 561, -1, 560, 361, 561, -1, 555, 361, 560, -1, 472, 359, 562, -1, 359, 361, 562, -1, 475, 472, 562, -1, 475, 558, 563, -1, 551, 564, 562, -1, 564, 565, 562, -1, 565, 566, 562, -1, 566, 567, 562, -1, 567, 568, 562, -1, 568, 569, 562, -1, 569, 570, 562, -1, 570, 557, 562, -1, 557, 475, 562, -1, 361, 551, 562, -1, 475, 563, 571, -1, 475, 571, 572, -1, 475, 572, 573, -1, 574, 575, 576, -1, 577, 574, 576, -1, 448, 441, 578, -1, 452, 448, 578, -1, 456, 452, 578, -1, 459, 456, 578, -1, 464, 459, 578, -1, 467, 464, 578, -1, 469, 467, 578, -1, 465, 469, 578, -1, 462, 465, 578, -1, 455, 462, 578, -1, 450, 455, 578, -1, 446, 450, 578, -1, 443, 446, 578, -1, 439, 443, 578, -1, 435, 439, 578, -1, 431, 435, 578, -1, 428, 431, 578, -1, 423, 428, 578, -1, 420, 423, 578, -1, 416, 420, 578, -1, 412, 416, 578, -1, 406, 412, 578, -1, 404, 406, 578, -1, 400, 404, 578, -1, 394, 400, 578, -1, 392, 394, 578, -1, 388, 392, 578, -1, 383, 388, 578, -1, 380, 383, 578, -1, 374, 380, 578, -1, 371, 374, 578, -1, 367, 371, 578, -1, 366, 367, 578, -1, 369, 366, 578, -1, 376, 369, 578, -1, 378, 376, 578, -1, 381, 378, 578, -1, 385, 381, 578, -1, 389, 385, 578, -1, 395, 389, 578, -1, 397, 395, 578, -1, 401, 397, 578, -1, 408, 401, 578, -1, 411, 408, 578, -1, 415, 411, 578, -1, 418, 415, 578, -1, 422, 418, 578, -1, 426, 422, 578, -1, 430, 426, 578, -1, 434, 430, 578, -1, 437, 434, 578, -1, 441, 437, 578, -1, 579, 580, 581, -1, 579, 582, 580, -1, 583, 550, 584, -1, 583, 549, 550, -1, 585, 584, 586, -1, 585, 583, 584, -1, 587, 586, 588, -1, 587, 585, 586, -1, 589, 588, 590, -1, 589, 587, 588, -1, 591, 590, 592, -1, 591, 589, 590, -1, 593, 592, 594, -1, 593, 591, 592, -1, 595, 594, 596, -1, 595, 593, 594, -1, 597, 596, 598, -1, 597, 595, 596, -1, 599, 598, 600, -1, 599, 597, 598, -1, 344, 600, 476, -1, 344, 599, 600, -1, 601, 602, 603, -1, 602, 604, 603, -1, 484, 485, 516, -1, 485, 518, 516, -1, 605, 601, 606, -1, 601, 603, 606, -1, 483, 484, 517, -1, 607, 608, 609, -1, 608, 610, 609, -1, 484, 516, 517, -1, 611, 605, 612, -1, 605, 606, 612, -1, 613, 607, 614, -1, 611, 612, 608, -1, 482, 483, 527, -1, 607, 609, 614, -1, 483, 517, 527, -1, 608, 612, 610, -1, 479, 482, 525, -1, 615, 613, 616, -1, 482, 527, 525, -1, 613, 614, 616, -1, 479, 525, 523, -1, 480, 479, 523, -1, 617, 615, 618, -1, 615, 616, 618, -1, 480, 523, 522, -1, 487, 480, 522, -1, 619, 617, 620, -1, 617, 618, 620, -1, 487, 522, 524, -1, 477, 487, 524, -1, 489, 619, 521, -1, 619, 620, 521, -1, 477, 524, 526, -1, 478, 477, 526, -1, 488, 489, 520, -1, 478, 526, 481, -1, 489, 521, 520, -1, 481, 526, 528, -1, 486, 488, 519, -1, 621, 481, 622, -1, 481, 528, 622, -1, 488, 520, 519, -1, 602, 621, 604, -1, 485, 486, 518, -1, 621, 622, 604, -1, 486, 519, 518, -1, 611, 608, 623, -1, 624, 611, 623, -1, 625, 278, 481, -1, 623, 608, 626, -1, 608, 607, 627, -1, 628, 625, 621, -1, 629, 628, 621, -1, 625, 481, 621, -1, 626, 608, 627, -1, 630, 629, 602, -1, 619, 489, 631, -1, 489, 265, 631, -1, 629, 621, 602, -1, 607, 613, 632, -1, 633, 630, 601, -1, 627, 607, 632, -1, 630, 602, 601, -1, 617, 619, 634, -1, 619, 631, 634, -1, 601, 605, 635, -1, 632, 613, 636, -1, 633, 601, 635, -1, 617, 634, 637, -1, 605, 611, 638, -1, 615, 617, 639, -1, 635, 605, 638, -1, 617, 637, 639, -1, 613, 615, 640, -1, 636, 613, 640, -1, 640, 615, 641, -1, 615, 639, 641, -1, 638, 611, 624, -1, 642, 574, 577, -1, 642, 643, 574, -1, 644, 645, 501, -1, 646, 507, 508, -1, 646, 508, 647, -1, 648, 511, 512, -1, 648, 644, 511, -1, 649, 505, 506, -1, 649, 506, 507, -1, 649, 507, 646, -1, 650, 504, 505, -1, 650, 505, 649, -1, 651, 491, 503, -1, 651, 503, 504, -1, 651, 504, 650, -1, 652, 491, 651, -1, 490, 491, 652, -1, 492, 652, 653, -1, 492, 490, 652, -1, 654, 494, 495, -1, 654, 495, 497, -1, 493, 653, 655, -1, 493, 492, 653, -1, 645, 497, 501, -1, 645, 654, 497, -1, 496, 493, 655, -1, 647, 508, 510, -1, 499, 655, 656, -1, 647, 510, 494, -1, 647, 494, 654, -1, 499, 496, 655, -1, 500, 499, 656, -1, 644, 501, 509, -1, 644, 509, 511, -1, 513, 529, 530, -1, 513, 530, 657, -1, 658, 610, 612, -1, 658, 612, 659, -1, 528, 660, 661, -1, 662, 610, 658, -1, 663, 609, 610, -1, 622, 661, 664, -1, 622, 664, 665, -1, 622, 528, 661, -1, 663, 610, 662, -1, 604, 665, 666, -1, 667, 521, 620, -1, 667, 668, 521, -1, 604, 622, 665, -1, 669, 614, 609, -1, 603, 666, 670, -1, 669, 609, 663, -1, 603, 604, 666, -1, 671, 620, 618, -1, 671, 667, 620, -1, 672, 606, 603, -1, 673, 614, 669, -1, 672, 603, 670, -1, 674, 671, 618, -1, 675, 612, 606, -1, 676, 618, 616, -1, 675, 606, 672, -1, 676, 674, 618, -1, 677, 616, 614, -1, 677, 614, 673, -1, 678, 616, 677, -1, 678, 676, 616, -1, 659, 612, 675, -1, 563, 679, 680, -1, 569, 568, 681, -1, 682, 569, 681, -1, 573, 572, 683, -1, 572, 680, 683, -1, 567, 566, 684, -1, 568, 567, 684, -1, 681, 568, 684, -1, 566, 565, 685, -1, 684, 566, 685, -1, 564, 551, 686, -1, 565, 564, 686, -1, 685, 565, 686, -1, 686, 551, 687, -1, 687, 551, 552, -1, 688, 687, 553, -1, 687, 552, 553, -1, 556, 557, 689, -1, 558, 556, 689, -1, 690, 688, 554, -1, 688, 553, 554, -1, 563, 558, 679, -1, 558, 689, 679, -1, 690, 554, 555, -1, 570, 569, 682, -1, 691, 690, 560, -1, 557, 570, 682, -1, 689, 557, 682, -1, 690, 555, 560, -1, 691, 560, 559, -1, 571, 563, 680, -1, 572, 571, 680, -1, 692, 580, 693, -1, 692, 581, 580, -1, 694, 693, 695, -1, 694, 692, 693, -1, 696, 695, 697, -1, 696, 694, 695, -1, 698, 697, 699, -1, 698, 696, 697, -1, 700, 699, 701, -1, 700, 698, 699, -1, 702, 701, 703, -1, 702, 700, 701, -1, 704, 703, 705, -1, 704, 702, 703, -1, 706, 705, 707, -1, 706, 704, 705, -1, 708, 707, 709, -1, 708, 706, 707, -1, 710, 709, 711, -1, 710, 708, 709, -1, 712, 711, 713, -1, 712, 710, 711, -1, 714, 713, 715, -1, 714, 712, 713, -1, 716, 715, 717, -1, 716, 714, 715, -1, 718, 717, 719, -1, 718, 716, 717, -1, 720, 719, 721, -1, 720, 718, 719, -1, 722, 721, 723, -1, 722, 720, 721, -1, 724, 723, 725, -1, 724, 722, 723, -1, 726, 725, 727, -1, 726, 724, 725, -1, 728, 727, 729, -1, 728, 726, 727, -1, 642, 729, 643, -1, 642, 728, 729, -1, 730, 731, 732, -1, 731, 733, 732, -1, 686, 687, 651, -1, 687, 652, 651, -1, 734, 730, 735, -1, 730, 732, 735, -1, 685, 686, 650, -1, 736, 737, 738, -1, 737, 739, 738, -1, 686, 651, 650, -1, 740, 734, 741, -1, 734, 735, 741, -1, 742, 736, 743, -1, 740, 741, 737, -1, 684, 685, 649, -1, 736, 738, 743, -1, 685, 650, 649, -1, 737, 741, 739, -1, 681, 684, 646, -1, 744, 742, 745, -1, 684, 649, 646, -1, 742, 743, 745, -1, 681, 646, 647, -1, 682, 681, 647, -1, 746, 744, 747, -1, 744, 745, 747, -1, 682, 647, 654, -1, 689, 682, 654, -1, 748, 746, 749, -1, 746, 747, 749, -1, 689, 654, 645, -1, 679, 689, 645, -1, 691, 748, 656, -1, 748, 749, 656, -1, 680, 679, 644, -1, 679, 645, 644, -1, 690, 691, 655, -1, 680, 644, 683, -1, 691, 656, 655, -1, 683, 644, 648, -1, 688, 690, 653, -1, 750, 683, 751, -1, 683, 648, 751, -1, 690, 655, 653, -1, 731, 750, 733, -1, 687, 688, 652, -1, 750, 751, 733, -1, 688, 653, 652, -1, 752, 739, 741, -1, 752, 741, 753, -1, 648, 512, 754, -1, 755, 739, 752, -1, 756, 738, 739, -1, 751, 754, 694, -1, 751, 694, 757, -1, 751, 648, 754, -1, 756, 739, 755, -1, 733, 757, 698, -1, 758, 656, 749, -1, 758, 500, 656, -1, 733, 751, 757, -1, 759, 743, 738, -1, 732, 698, 760, -1, 759, 738, 756, -1, 732, 733, 698, -1, 761, 749, 747, -1, 761, 758, 749, -1, 762, 735, 732, -1, 763, 743, 759, -1, 762, 732, 760, -1, 764, 761, 747, -1, 765, 741, 735, -1, 766, 747, 745, -1, 765, 735, 762, -1, 766, 764, 747, -1, 767, 745, 743, -1, 767, 743, 763, -1, 768, 745, 767, -1, 768, 766, 745, -1, 753, 741, 765, -1, 740, 737, 769, -1, 770, 740, 769, -1, 771, 573, 683, -1, 769, 737, 772, -1, 737, 736, 773, -1, 695, 771, 750, -1, 774, 695, 750, -1, 771, 683, 750, -1, 772, 737, 773, -1, 699, 774, 731, -1, 748, 691, 775, -1, 691, 559, 775, -1, 774, 750, 731, -1, 736, 742, 776, -1, 701, 699, 730, -1, 773, 736, 776, -1, 699, 731, 730, -1, 746, 748, 777, -1, 748, 775, 777, -1, 730, 734, 703, -1, 776, 742, 778, -1, 701, 730, 703, -1, 746, 777, 779, -1, 734, 740, 780, -1, 744, 746, 781, -1, 703, 734, 780, -1, 746, 779, 781, -1, 742, 744, 782, -1, 778, 742, 782, -1, 782, 744, 783, -1, 744, 781, 783, -1, 780, 740, 770, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 13.611 3.975 65.435 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 13.611 3.975 -116.759 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 13.611 -87.122 -25.662 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 13.611 95.072 -25.662 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -77.487 3.975 -25.662 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 104.708 3.975 -25.662 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_HIP_R.wrl000066400000000000000000000740341207742442300227470ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 46.500 27.000 43.000 center 0.000 0.000 0.000 #translation -12.760 -0.500 9.520 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.60 0.30 0.30 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -9.490 -12.000 11.980, 14.010 -12.000 11.980, 14.010 11.000 11.980, -9.490 11.000 11.980, 14.010 11.000 11.980, 14.010 -12.000 11.980, 14.010 -12.000 6.980, 14.010 11.000 6.980, -9.490 -12.000 -26.020, 2.260 -12.000 -7.020, 14.010 -12.000 -26.020, -9.490 -8.245 -0.025, -9.490 1.895 -5.653, -9.490 3.382 -5.015, -9.490 11.000 -26.020, -9.490 5.298 7.492, -9.490 4.070 8.546, -9.490 11.000 11.980, -9.490 4.711 -4.090, -9.490 2.655 9.332, -9.490 5.826 -2.917, -9.490 1.110 9.816, -9.490 6.682 -1.543, -9.490 -0.500 9.980, -9.490 -6.298 7.492, -9.490 -5.070 8.546, -9.490 7.245 -0.025, -9.490 7.490 1.575, -9.490 6.289 6.212, -9.490 -2.895 -5.653, -9.490 -1.309 -5.979, -9.490 -0.500 -7.020, -9.490 0.309 -5.979, -9.490 -7.289 6.212, -9.490 7.002 4.758, -9.490 -3.655 9.332, -9.490 -8.002 4.758, -9.490 -2.110 9.816, -9.490 7.408 3.191, -9.490 -8.408 3.191, -9.490 -8.490 1.575, -9.490 -7.682 -1.543, -9.490 -6.826 -2.917, -9.490 -5.711 -4.090, -9.490 -4.382 -5.015, 2.260 11.000 -7.020, 14.010 11.000 -26.020, 14.010 11.000 6.980, 18.000 13.000 6.980, 14.010 13.000 6.980, 24.510 0.500 6.980, 35.010 -12.000 6.980, 35.010 13.000 6.980, 14.010 -12.000 6.980, 29.000 13.000 6.980, 35.010 -12.000 6.980, 31.755 -12.000 -5.025, 35.010 -12.000 -31.020, 21.615 -12.000 -10.653, 20.128 -12.000 -10.015, 18.212 -12.000 2.492, 17.221 -12.000 1.212, 18.799 -12.000 -9.090, 29.808 -12.000 2.492, 28.580 -12.000 3.546, 17.684 -12.000 -7.917, 16.828 -12.000 -6.543, 30.799 -12.000 1.212, 14.010 -12.000 -31.020, 16.265 -12.000 -5.025, 16.020 -12.000 -3.425, 27.165 -12.000 4.332, 27.892 -12.000 -10.015, 26.405 -12.000 -10.653, 24.510 -12.000 -12.020, 24.819 -12.000 -10.979, 16.508 -12.000 -0.242, 23.201 -12.000 -10.979, 31.512 -12.000 -0.242, 25.620 -12.000 4.816, 16.102 -12.000 -1.809, 31.918 -12.000 -1.809, 24.010 -12.000 4.980, 32.000 -12.000 -3.425, 19.440 -12.000 3.546, 20.855 -12.000 4.332, 22.400 -12.000 4.816, 31.192 -12.000 -6.543, 30.336 -12.000 -7.917, 29.221 -12.000 -9.090, -10.490 -2.110 9.816, -10.490 -0.500 9.980, -10.490 -3.655 9.332, -10.490 -5.070 8.546, -10.490 -6.298 7.492, -10.490 -7.289 6.212, -10.490 -8.002 4.758, -10.490 -8.408 3.191, -10.490 -8.490 1.575, -10.490 -8.245 -0.025, -10.490 -7.682 -1.543, -10.490 -6.826 -2.917, -10.490 -5.711 -4.090, -10.490 -4.382 -5.015, -10.490 -2.895 -5.653, -10.490 -1.309 -5.979, -10.490 0.309 -5.979, -10.490 1.895 -5.653, -10.490 3.382 -5.015, -10.490 4.711 -4.090, -10.490 5.826 -2.917, -10.490 6.682 -1.543, -10.490 7.245 -0.025, -10.490 7.490 1.575, -10.490 7.408 3.191, -10.490 7.002 4.758, -10.490 6.289 6.212, -10.490 5.298 7.492, -10.490 4.070 8.546, -10.490 2.655 9.332, -10.490 1.110 9.816, 14.010 13.000 6.980, 14.010 13.000 -31.020, 35.010 -3.877 4.123, 35.010 -3.238 4.896, 35.010 -2.427 5.485, 35.010 -4.304 3.216, 35.010 -1.495 5.854, 35.010 -4.492 2.231, 35.010 13.000 6.980, 35.010 3.119 0.277, 35.010 2.582 -0.570, 35.010 3.429 1.230, 35.010 3.492 2.231, 35.010 3.304 3.216, 35.010 2.877 4.123, 35.010 2.238 4.896, 35.010 -0.500 5.980, 35.010 1.427 5.485, 35.010 0.495 5.854, 35.010 -4.429 1.230, 35.010 -3.582 -0.570, 35.010 -4.119 0.277, 35.010 0.500 -12.020, 35.010 -2.851 -1.256, 35.010 -1.972 -1.739, 35.010 -1.001 -1.988, 35.010 0.001 -1.988, 35.010 0.972 -1.739, 35.010 1.851 -1.256, 35.010 13.000 -31.020, 35.010 13.000 -31.020, 29.000 13.000 -3.000, 27.464 13.000 -5.000, 14.010 13.000 -31.020, 18.000 13.000 -3.000, 21.172 13.000 -5.828, 24.510 13.000 -12.020, 22.000 13.000 -6.464, 22.965 13.000 -6.864, 24.000 13.000 -7.000, 25.035 13.000 -6.864, 26.000 13.000 -6.464, 26.828 13.000 -5.828, 27.864 13.000 -4.035, 28.000 13.000 -3.000, 20.000 13.000 -3.000, 20.136 13.000 -4.035, 20.536 13.000 -5.000, 23.832 13.000 5.976, 22.832 13.000 5.803, 21.908 13.000 5.383, 21.119 13.000 4.745, 20.517 13.000 3.928, 27.316 13.000 4.232, 26.643 13.000 4.991, 25.801 13.000 5.557, 20.139 13.000 2.987, 27.776 13.000 3.327, 20.010 13.000 1.980, 24.843 13.000 5.892, 27.994 13.000 2.336, 20.397 13.000 -1.263, 20.402 13.000 0.252, 20.875 13.000 -0.504, 27.608 13.000 -1.272, 27.135 13.000 -0.516, 27.663 13.000 0.351, 20.101 13.000 -2.109, 27.956 13.000 1.322, 20.109 13.000 1.094, 27.901 13.000 -2.114, 25.620 -13.000 4.816, 24.010 -13.000 4.980, 27.165 -13.000 4.332, 28.580 -13.000 3.546, 29.808 -13.000 2.492, 30.799 -13.000 1.212, 31.512 -13.000 -0.242, 31.918 -13.000 -1.809, 32.000 -13.000 -3.425, 31.755 -13.000 -5.025, 31.192 -13.000 -6.543, 30.336 -13.000 -7.917, 29.221 -13.000 -9.090, 27.892 -13.000 -10.015, 26.405 -13.000 -10.653, 24.819 -13.000 -10.979, 23.201 -13.000 -10.979, 21.615 -13.000 -10.653, 20.128 -13.000 -10.015, 18.799 -13.000 -9.090, 17.684 -13.000 -7.917, 16.828 -13.000 -6.543, 16.265 -13.000 -5.025, 16.020 -13.000 -3.425, 16.102 -13.000 -1.809, 16.508 -13.000 -0.242, 17.221 -13.000 1.212, 18.212 -13.000 2.492, 19.440 -13.000 3.546, 20.855 -13.000 4.332, 22.400 -13.000 4.816, 35.010 13.000 -31.020, 14.010 -12.000 -31.020, 14.010 13.000 -31.020, -10.490 5.826 -2.917, -10.490 4.711 -4.090, -10.490 0.271 1.343, -10.490 3.382 -5.015, -10.490 0.088 1.171, -10.490 6.682 -1.543, -10.490 0.405 1.554, -10.490 7.245 -0.025, -10.490 1.895 -5.653, -10.490 -0.132 1.050, -10.490 7.490 1.575, -10.490 0.482 1.793, -10.490 0.309 -5.979, -10.490 -0.375 0.988, -10.490 0.498 2.043, -10.490 -1.309 -5.979, -10.490 -2.895 -5.653, -10.490 -0.625 0.988, -10.490 7.002 4.758, -10.490 0.451 2.289, -10.490 -0.868 1.050, -10.490 0.344 2.516, -10.490 5.298 7.492, -10.490 -1.088 1.171, -10.490 -5.711 -4.090, -10.490 0.185 2.709, -10.490 -1.271 1.343, -10.490 -1.405 1.554, -10.490 -6.826 -2.917, -10.490 4.070 8.546, -10.490 -0.018 2.856, -10.490 -7.682 -1.543, -10.490 -0.251 2.949, -10.490 -1.482 1.793, -10.490 -8.245 -0.025, -10.490 -0.500 2.980, -10.490 1.110 9.816, -10.490 -1.498 2.043, -10.490 -8.490 1.575, -10.490 -0.500 9.980, -10.490 -1.451 2.289, -10.490 -8.408 3.191, -10.490 -0.749 2.949, -10.490 -2.110 9.816, -10.490 -1.344 2.516, -10.490 -8.002 4.758, -10.490 -0.982 2.856, -10.490 -3.655 9.332, -10.490 -7.289 6.212, -10.490 -1.185 2.709, -10.490 -5.070 8.546, -10.490 -6.298 7.492, 36.010 -1.495 5.854, 36.010 -0.500 5.980, 36.010 -2.427 5.485, 36.010 -3.238 4.896, 36.010 -3.877 4.123, 36.010 -4.304 3.216, 36.010 -4.492 2.231, 36.010 -4.429 1.230, 36.010 -4.119 0.277, 36.010 -3.582 -0.570, 36.010 -2.851 -1.256, 36.010 -1.972 -1.739, 36.010 -1.001 -1.988, 36.010 0.001 -1.988, 36.010 0.972 -1.739, 36.010 1.851 -1.256, 36.010 2.582 -0.570, 36.010 3.119 0.277, 36.010 3.429 1.230, 36.010 3.492 2.231, 36.010 3.304 3.216, 36.010 2.877 4.123, 36.010 2.238 4.896, 36.010 1.427 5.485, 36.010 0.495 5.854, 27.936 14.000 -2.286, 28.000 14.000 -3.000, 27.745 14.000 -1.595, 27.434 14.000 -0.948, 27.012 14.000 -0.368, 26.402 13.000 0.199, 26.494 14.000 0.127, 25.895 14.000 0.522, 25.511 13.000 0.704, 25.236 14.000 0.804, 24.520 13.000 0.966, 24.537 14.000 0.964, 23.821 14.000 0.996, 23.496 13.000 0.968, 23.110 14.000 0.900, 22.504 13.000 0.710, 22.428 14.000 0.678, 21.796 14.000 0.338, 21.611 13.000 0.208, 21.236 14.000 -0.109, 20.764 14.000 -0.649, 20.396 14.000 -1.264, 20.144 14.000 -1.936, 20.016 14.000 -2.641, 20.016 14.000 -3.359, 20.144 14.000 -4.064, 20.396 14.000 -4.736, 20.764 14.000 -5.351, 21.236 14.000 -5.891, 21.796 14.000 -6.338, 22.428 14.000 -6.678, 23.110 14.000 -6.900, 23.821 14.000 -6.996, 24.537 14.000 -6.964, 25.236 14.000 -6.804, 25.895 14.000 -6.522, 26.494 14.000 -6.127, 27.012 14.000 -5.632, 27.434 14.000 -5.052, 27.745 14.000 -4.405, 27.936 14.000 -3.714, 24.002 13.000 2.730, 17.684 -13.000 -7.917, 18.799 -13.000 -9.090, 23.239 -13.000 -3.657, 20.128 -13.000 -10.015, 23.422 -13.000 -3.829, 16.828 -13.000 -6.543, 23.105 -13.000 -3.446, 16.265 -13.000 -5.025, 21.615 -13.000 -10.653, 23.642 -13.000 -3.950, 16.020 -13.000 -3.425, 23.028 -13.000 -3.207, 23.201 -13.000 -10.979, 23.885 -13.000 -4.012, 16.102 -13.000 -1.809, 23.012 -13.000 -2.957, 24.819 -13.000 -10.979, 26.405 -13.000 -10.653, 24.135 -13.000 -4.012, 16.508 -13.000 -0.242, 23.059 -13.000 -2.711, 27.892 -13.000 -10.015, 24.378 -13.000 -3.950, 23.166 -13.000 -2.484, 17.221 -13.000 1.212, 18.212 -13.000 2.492, 24.598 -13.000 -3.829, 29.221 -13.000 -9.090, 23.325 -13.000 -2.291, 24.781 -13.000 -3.657, 24.915 -13.000 -3.446, 30.336 -13.000 -7.917, 19.440 -13.000 3.546, 23.528 -13.000 -2.144, 31.192 -13.000 -6.543, 20.855 -13.000 4.332, 23.761 -13.000 -2.051, 24.992 -13.000 -3.207, 31.755 -13.000 -5.025, 24.010 -13.000 -2.020, 22.400 -13.000 4.816, 25.008 -13.000 -2.957, 32.000 -13.000 -3.425, 24.010 -13.000 4.980, 24.961 -13.000 -2.711, 31.918 -13.000 -1.809, 24.259 -13.000 -2.051, 25.620 -13.000 4.816, 24.854 -13.000 -2.484, 31.512 -13.000 -0.242, 24.492 -13.000 -2.144, 27.165 -13.000 4.332, 30.799 -13.000 1.212, 24.695 -13.000 -2.291, 28.580 -13.000 3.546, 29.808 -13.000 2.492, -10.490 -0.500 1.984, 36.010 -0.500 1.996, 36.010 -0.500 5.980, 36.010 -1.495 5.854, 36.010 -2.427 5.485, 36.010 -3.238 4.896, 36.010 -3.877 4.123, 36.010 -4.304 3.216, 36.010 -4.492 2.231, 36.010 -4.429 1.230, 36.010 -4.119 0.277, 36.010 -3.582 -0.570, 36.010 -2.851 -1.256, 36.010 -1.972 -1.739, 36.010 -1.001 -1.988, 36.010 0.001 -1.988, 36.010 0.972 -1.739, 36.010 1.851 -1.256, 36.010 2.582 -0.570, 36.010 3.119 0.277, 36.010 3.429 1.230, 36.010 3.492 2.231, 36.010 3.304 3.216, 36.010 2.877 4.123, 36.010 2.238 4.896, 36.010 1.427 5.485, 36.010 0.495 5.854, 24.008 14.000 -3.000, 25.236 14.000 -6.804, 27.936 14.000 -2.286, 24.010 -13.000 -3.016 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 0, 2, 3, -1, 4, 5, 6, -1, 7, 4, 6, -1, 6, 5, 0, -1, 0, 8, 9, -1, 8, 10, 9, -1, 10, 6, 9, -1, 6, 0, 9, -1, 0, 11, 8, -1, 12, 13, 14, -1, 15, 16, 17, -1, 13, 18, 14, -1, 16, 19, 17, -1, 18, 20, 14, -1, 19, 21, 17, -1, 20, 22, 14, -1, 21, 23, 17, -1, 23, 0, 17, -1, 24, 0, 25, -1, 14, 22, 26, -1, 27, 14, 26, -1, 15, 17, 28, -1, 17, 14, 27, -1, 29, 30, 31, -1, 30, 32, 31, -1, 0, 24, 33, -1, 32, 12, 31, -1, 14, 8, 31, -1, 12, 14, 31, -1, 8, 29, 31, -1, 28, 17, 34, -1, 25, 0, 35, -1, 0, 33, 36, -1, 35, 0, 37, -1, 34, 17, 38, -1, 0, 36, 39, -1, 38, 17, 27, -1, 37, 0, 23, -1, 0, 39, 40, -1, 0, 40, 11, -1, 11, 41, 8, -1, 41, 42, 8, -1, 42, 43, 8, -1, 43, 44, 8, -1, 44, 29, 8, -1, 17, 4, 7, -1, 45, 14, 17, -1, 45, 46, 14, -1, 45, 7, 46, -1, 45, 17, 7, -1, 47, 48, 49, -1, 50, 51, 52, -1, 50, 53, 51, -1, 50, 52, 54, -1, 50, 47, 53, -1, 50, 54, 48, -1, 50, 48, 47, -1, 46, 10, 14, -1, 14, 10, 8, -1, 55, 56, 57, -1, 58, 59, 10, -1, 60, 6, 61, -1, 59, 62, 10, -1, 63, 55, 64, -1, 62, 65, 10, -1, 65, 66, 10, -1, 55, 63, 67, -1, 57, 10, 68, -1, 10, 66, 69, -1, 70, 10, 69, -1, 64, 55, 71, -1, 6, 10, 70, -1, 72, 73, 74, -1, 73, 75, 74, -1, 61, 6, 76, -1, 75, 77, 74, -1, 77, 58, 74, -1, 10, 57, 74, -1, 57, 72, 74, -1, 55, 67, 78, -1, 58, 10, 74, -1, 71, 55, 79, -1, 76, 6, 80, -1, 55, 78, 81, -1, 79, 55, 82, -1, 80, 6, 70, -1, 55, 81, 83, -1, 55, 83, 56, -1, 84, 85, 6, -1, 85, 86, 6, -1, 86, 82, 6, -1, 56, 87, 57, -1, 87, 88, 57, -1, 88, 89, 57, -1, 82, 55, 6, -1, 89, 72, 57, -1, 84, 6, 60, -1, 37, 23, 90, -1, 23, 91, 90, -1, 37, 90, 35, -1, 35, 90, 92, -1, 25, 35, 93, -1, 35, 92, 93, -1, 24, 25, 94, -1, 25, 93, 94, -1, 33, 24, 95, -1, 24, 94, 95, -1, 36, 33, 96, -1, 33, 95, 96, -1, 39, 36, 97, -1, 36, 96, 97, -1, 40, 39, 98, -1, 39, 97, 98, -1, 40, 98, 99, -1, 11, 40, 99, -1, 11, 99, 100, -1, 41, 11, 100, -1, 41, 100, 101, -1, 42, 41, 101, -1, 43, 42, 102, -1, 42, 101, 102, -1, 44, 43, 103, -1, 43, 102, 103, -1, 29, 44, 104, -1, 44, 103, 104, -1, 30, 29, 105, -1, 29, 104, 105, -1, 32, 30, 106, -1, 30, 105, 106, -1, 12, 32, 107, -1, 32, 106, 107, -1, 13, 12, 108, -1, 12, 107, 108, -1, 18, 13, 109, -1, 13, 108, 109, -1, 20, 18, 110, -1, 18, 109, 110, -1, 22, 20, 111, -1, 20, 110, 111, -1, 26, 22, 112, -1, 22, 111, 112, -1, 27, 26, 113, -1, 26, 112, 113, -1, 38, 27, 114, -1, 27, 113, 114, -1, 34, 38, 115, -1, 38, 114, 115, -1, 28, 34, 116, -1, 34, 115, 116, -1, 15, 28, 117, -1, 28, 116, 117, -1, 15, 117, 16, -1, 16, 117, 118, -1, 19, 16, 119, -1, 16, 118, 119, -1, 19, 119, 21, -1, 21, 119, 120, -1, 23, 21, 91, -1, 21, 120, 91, -1, 68, 10, 46, -1, 7, 121, 46, -1, 68, 46, 122, -1, 46, 121, 122, -1, 123, 124, 55, -1, 125, 55, 124, -1, 126, 123, 55, -1, 127, 55, 125, -1, 128, 126, 55, -1, 129, 130, 131, -1, 129, 132, 130, -1, 129, 133, 132, -1, 129, 134, 133, -1, 129, 135, 134, -1, 129, 136, 135, -1, 137, 55, 127, -1, 129, 138, 136, -1, 129, 139, 138, -1, 129, 137, 139, -1, 140, 128, 55, -1, 57, 141, 55, -1, 142, 140, 55, -1, 129, 55, 137, -1, 143, 144, 141, -1, 143, 145, 144, -1, 143, 146, 145, -1, 143, 147, 146, -1, 143, 148, 147, -1, 143, 149, 148, -1, 143, 131, 149, -1, 143, 150, 129, -1, 143, 57, 150, -1, 143, 129, 131, -1, 143, 141, 57, -1, 141, 142, 55, -1, 151, 152, 52, -1, 151, 153, 152, -1, 152, 54, 52, -1, 154, 155, 156, -1, 154, 49, 155, -1, 157, 156, 158, -1, 157, 158, 159, -1, 157, 159, 160, -1, 157, 160, 161, -1, 157, 161, 162, -1, 157, 162, 163, -1, 164, 165, 152, -1, 157, 163, 153, -1, 157, 151, 154, -1, 157, 153, 151, -1, 157, 154, 156, -1, 153, 164, 152, -1, 155, 166, 167, -1, 155, 167, 168, -1, 155, 168, 156, -1, 155, 49, 48, -1, 48, 169, 170, -1, 48, 170, 171, -1, 48, 171, 172, -1, 48, 54, 169, -1, 173, 48, 172, -1, 174, 175, 54, -1, 176, 54, 175, -1, 177, 48, 173, -1, 178, 174, 54, -1, 179, 48, 177, -1, 180, 54, 176, -1, 181, 178, 54, -1, 169, 54, 180, -1, 182, 183, 184, -1, 185, 186, 187, -1, 155, 182, 188, -1, 152, 187, 189, -1, 155, 188, 166, -1, 152, 189, 181, -1, 155, 179, 190, -1, 155, 190, 183, -1, 152, 181, 54, -1, 155, 183, 182, -1, 152, 185, 187, -1, 155, 48, 179, -1, 191, 185, 152, -1, 165, 191, 152, -1, 79, 82, 192, -1, 82, 193, 192, -1, 79, 192, 71, -1, 71, 192, 194, -1, 64, 71, 195, -1, 71, 194, 195, -1, 63, 64, 196, -1, 64, 195, 196, -1, 67, 63, 197, -1, 63, 196, 197, -1, 78, 67, 198, -1, 67, 197, 198, -1, 81, 78, 199, -1, 78, 198, 199, -1, 83, 81, 200, -1, 81, 199, 200, -1, 83, 200, 201, -1, 56, 83, 201, -1, 56, 201, 202, -1, 87, 56, 202, -1, 87, 202, 203, -1, 88, 87, 203, -1, 89, 88, 204, -1, 88, 203, 204, -1, 72, 89, 205, -1, 89, 204, 205, -1, 73, 72, 206, -1, 72, 205, 206, -1, 75, 73, 207, -1, 73, 206, 207, -1, 77, 75, 208, -1, 75, 207, 208, -1, 58, 77, 209, -1, 77, 208, 209, -1, 59, 58, 210, -1, 58, 209, 210, -1, 62, 59, 211, -1, 59, 210, 211, -1, 65, 62, 212, -1, 62, 211, 212, -1, 66, 65, 213, -1, 65, 212, 213, -1, 69, 66, 214, -1, 66, 213, 214, -1, 70, 69, 215, -1, 69, 214, 215, -1, 80, 70, 216, -1, 70, 215, 216, -1, 76, 80, 217, -1, 80, 216, 217, -1, 61, 76, 218, -1, 76, 217, 218, -1, 60, 61, 219, -1, 61, 218, 219, -1, 60, 219, 84, -1, 84, 219, 220, -1, 85, 84, 221, -1, 84, 220, 221, -1, 85, 221, 86, -1, 86, 221, 222, -1, 82, 86, 193, -1, 86, 222, 193, -1, 223, 57, 224, -1, 225, 223, 224, -1, 226, 227, 228, -1, 227, 229, 230, -1, 228, 227, 230, -1, 231, 226, 232, -1, 233, 231, 232, -1, 226, 228, 232, -1, 229, 234, 235, -1, 230, 229, 235, -1, 236, 233, 237, -1, 233, 232, 237, -1, 234, 238, 239, -1, 235, 234, 239, -1, 114, 236, 240, -1, 236, 237, 240, -1, 241, 242, 243, -1, 238, 241, 243, -1, 239, 238, 243, -1, 244, 114, 245, -1, 114, 240, 245, -1, 242, 103, 246, -1, 243, 242, 246, -1, 244, 245, 247, -1, 116, 244, 247, -1, 116, 247, 248, -1, 246, 103, 249, -1, 103, 250, 249, -1, 248, 247, 251, -1, 249, 250, 252, -1, 253, 252, 254, -1, 252, 250, 254, -1, 248, 251, 255, -1, 251, 256, 255, -1, 253, 254, 257, -1, 255, 256, 119, -1, 256, 258, 119, -1, 259, 253, 260, -1, 253, 257, 260, -1, 258, 261, 262, -1, 119, 258, 262, -1, 263, 259, 264, -1, 259, 260, 264, -1, 262, 261, 265, -1, 266, 263, 267, -1, 263, 264, 267, -1, 261, 268, 269, -1, 265, 261, 269, -1, 270, 266, 271, -1, 266, 267, 271, -1, 268, 272, 273, -1, 269, 268, 273, -1, 270, 271, 274, -1, 272, 275, 276, -1, 273, 272, 276, -1, 275, 270, 277, -1, 276, 275, 277, -1, 270, 274, 277, -1, 278, 279, 127, -1, 279, 137, 127, -1, 280, 278, 125, -1, 278, 127, 125, -1, 281, 280, 124, -1, 280, 125, 124, -1, 282, 281, 123, -1, 281, 124, 123, -1, 283, 282, 126, -1, 282, 123, 126, -1, 284, 283, 128, -1, 283, 126, 128, -1, 285, 284, 140, -1, 284, 128, 140, -1, 286, 285, 142, -1, 285, 140, 142, -1, 287, 286, 141, -1, 286, 142, 141, -1, 288, 287, 144, -1, 287, 141, 144, -1, 289, 288, 145, -1, 288, 144, 145, -1, 289, 145, 146, -1, 290, 289, 146, -1, 290, 146, 147, -1, 291, 290, 147, -1, 291, 147, 148, -1, 292, 291, 148, -1, 292, 148, 149, -1, 293, 292, 149, -1, 294, 293, 131, -1, 293, 149, 131, -1, 295, 294, 130, -1, 294, 131, 130, -1, 296, 295, 132, -1, 295, 130, 132, -1, 297, 296, 133, -1, 296, 132, 133, -1, 298, 297, 134, -1, 297, 133, 134, -1, 298, 134, 299, -1, 299, 134, 135, -1, 300, 299, 136, -1, 299, 135, 136, -1, 300, 136, 301, -1, 301, 136, 138, -1, 302, 301, 139, -1, 301, 138, 139, -1, 279, 302, 137, -1, 302, 139, 137, -1, 303, 165, 304, -1, 191, 165, 303, -1, 305, 191, 303, -1, 185, 191, 305, -1, 306, 185, 305, -1, 186, 185, 306, -1, 307, 186, 306, -1, 308, 307, 309, -1, 308, 186, 307, -1, 310, 308, 309, -1, 311, 308, 310, -1, 312, 311, 310, -1, 313, 312, 314, -1, 313, 311, 312, -1, 315, 313, 314, -1, 316, 313, 315, -1, 317, 316, 315, -1, 318, 316, 317, -1, 319, 318, 317, -1, 320, 318, 319, -1, 321, 318, 320, -1, 322, 321, 320, -1, 184, 321, 322, -1, 323, 184, 322, -1, 182, 184, 323, -1, 182, 323, 324, -1, 325, 182, 324, -1, 188, 182, 325, -1, 326, 188, 325, -1, 166, 188, 326, -1, 327, 166, 326, -1, 167, 327, 328, -1, 167, 166, 327, -1, 329, 167, 328, -1, 168, 167, 329, -1, 330, 168, 329, -1, 156, 168, 330, -1, 331, 156, 330, -1, 332, 156, 331, -1, 158, 156, 332, -1, 333, 158, 332, -1, 159, 158, 333, -1, 334, 159, 333, -1, 335, 159, 334, -1, 160, 159, 335, -1, 336, 160, 335, -1, 161, 160, 336, -1, 337, 161, 336, -1, 162, 337, 338, -1, 162, 161, 337, -1, 339, 162, 338, -1, 163, 162, 339, -1, 340, 163, 339, -1, 153, 340, 341, -1, 153, 163, 340, -1, 342, 153, 341, -1, 164, 153, 342, -1, 343, 164, 342, -1, 165, 343, 304, -1, 165, 164, 343, -1, 181, 189, 311, -1, 321, 183, 190, -1, 321, 184, 183, -1, 178, 181, 311, -1, 318, 190, 179, -1, 318, 179, 177, -1, 318, 321, 190, -1, 344, 311, 313, -1, 344, 313, 316, -1, 344, 316, 318, -1, 344, 174, 178, -1, 344, 175, 174, -1, 344, 176, 175, -1, 344, 180, 176, -1, 344, 169, 180, -1, 344, 170, 169, -1, 344, 171, 170, -1, 344, 172, 171, -1, 344, 173, 172, -1, 344, 177, 173, -1, 344, 318, 177, -1, 344, 178, 311, -1, 187, 186, 308, -1, 189, 308, 311, -1, 189, 187, 308, -1, 345, 346, 347, -1, 346, 348, 349, -1, 347, 346, 349, -1, 350, 345, 351, -1, 352, 350, 351, -1, 345, 347, 351, -1, 348, 353, 354, -1, 349, 348, 354, -1, 355, 352, 356, -1, 352, 351, 356, -1, 353, 357, 358, -1, 354, 353, 358, -1, 359, 355, 360, -1, 355, 356, 360, -1, 361, 362, 363, -1, 357, 361, 363, -1, 358, 357, 363, -1, 364, 359, 365, -1, 359, 360, 365, -1, 362, 366, 367, -1, 363, 362, 367, -1, 364, 365, 368, -1, 369, 364, 368, -1, 369, 368, 370, -1, 367, 366, 371, -1, 366, 372, 371, -1, 370, 368, 373, -1, 371, 372, 374, -1, 375, 374, 376, -1, 374, 372, 376, -1, 370, 373, 377, -1, 373, 378, 377, -1, 375, 376, 379, -1, 377, 378, 380, -1, 378, 381, 380, -1, 382, 375, 383, -1, 375, 379, 383, -1, 381, 384, 385, -1, 380, 381, 385, -1, 386, 382, 387, -1, 382, 383, 387, -1, 385, 384, 388, -1, 389, 386, 390, -1, 386, 387, 390, -1, 384, 391, 392, -1, 388, 384, 392, -1, 393, 389, 394, -1, 389, 390, 394, -1, 391, 395, 396, -1, 392, 391, 396, -1, 393, 394, 397, -1, 395, 398, 399, -1, 396, 395, 399, -1, 398, 393, 400, -1, 399, 398, 400, -1, 393, 397, 400, -1, 268, 261, 401, -1, 272, 268, 401, -1, 275, 272, 401, -1, 270, 275, 401, -1, 266, 270, 401, -1, 263, 266, 401, -1, 259, 263, 401, -1, 253, 259, 401, -1, 252, 253, 401, -1, 249, 252, 401, -1, 246, 249, 401, -1, 243, 246, 401, -1, 239, 243, 401, -1, 235, 239, 401, -1, 230, 235, 401, -1, 228, 230, 401, -1, 232, 228, 401, -1, 237, 232, 401, -1, 240, 237, 401, -1, 245, 240, 401, -1, 247, 245, 401, -1, 251, 247, 401, -1, 256, 251, 401, -1, 258, 256, 401, -1, 261, 258, 401, -1, 402, 403, 404, -1, 402, 404, 405, -1, 402, 405, 406, -1, 402, 406, 407, -1, 402, 407, 408, -1, 402, 408, 409, -1, 402, 409, 410, -1, 402, 410, 411, -1, 402, 411, 412, -1, 402, 412, 413, -1, 402, 413, 414, -1, 402, 414, 415, -1, 402, 415, 416, -1, 402, 416, 417, -1, 402, 417, 418, -1, 402, 418, 419, -1, 402, 419, 420, -1, 402, 420, 421, -1, 402, 421, 422, -1, 402, 422, 423, -1, 402, 423, 424, -1, 402, 424, 425, -1, 402, 425, 426, -1, 402, 426, 427, -1, 402, 427, 403, -1, 324, 323, 428, -1, 325, 324, 428, -1, 326, 325, 428, -1, 327, 326, 428, -1, 328, 327, 428, -1, 329, 328, 428, -1, 330, 329, 428, -1, 331, 330, 428, -1, 332, 331, 428, -1, 333, 332, 428, -1, 334, 333, 428, -1, 335, 334, 428, -1, 336, 335, 428, -1, 429, 336, 428, -1, 338, 429, 428, -1, 339, 338, 428, -1, 340, 339, 428, -1, 341, 340, 428, -1, 342, 341, 428, -1, 343, 342, 428, -1, 304, 343, 428, -1, 430, 304, 428, -1, 305, 430, 428, -1, 306, 305, 428, -1, 307, 306, 428, -1, 309, 307, 428, -1, 310, 309, 428, -1, 312, 310, 428, -1, 314, 312, 428, -1, 315, 314, 428, -1, 317, 315, 428, -1, 319, 317, 428, -1, 320, 319, 428, -1, 322, 320, 428, -1, 323, 322, 428, -1, 391, 384, 431, -1, 395, 391, 431, -1, 398, 395, 431, -1, 393, 398, 431, -1, 389, 393, 431, -1, 386, 389, 431, -1, 382, 386, 431, -1, 375, 382, 431, -1, 374, 375, 431, -1, 371, 374, 431, -1, 367, 371, 431, -1, 363, 367, 431, -1, 358, 363, 431, -1, 354, 358, 431, -1, 349, 354, 431, -1, 347, 349, 431, -1, 351, 347, 431, -1, 356, 351, 431, -1, 360, 356, 431, -1, 365, 360, 431, -1, 368, 365, 431, -1, 373, 368, 431, -1, 378, 373, 431, -1, 381, 378, 431, -1, 384, 381, 431, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 12.760 0.500 59.329 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 12.760 0.500 -78.369 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 12.760 -68.349 -9.520 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 12.760 69.349 -9.520 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -56.089 0.500 -9.520 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 81.609 0.500 -9.520 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_HIP_Y.wrl000066400000000000000000002117751207742442300227630ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 63.000 41.997 37.000 center 0.000 0.000 0.000 #translation -10.500 0.001 20.500 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.50 0.50 0.50 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -14.000 -1.132 -31.924, -14.000 -0.956 -34.932, -14.000 -2.000 -32.000, -14.000 0.071 -34.727, -14.000 -5.214 -30.830, -14.000 -7.657 -32.657, -14.000 -5.830 -30.214, -14.000 -6.870 -33.347, -14.000 -4.500 -31.330, -14.000 -6.000 -33.928, -14.000 4.000 -27.000, -14.000 2.924 -27.868, -14.000 3.000 -27.000, -14.000 5.727 -29.071, -14.000 -5.061 -34.391, -14.000 -3.710 -31.698, -14.000 -4.071 -34.727, -14.000 -0.290 -31.698, -14.000 1.061 -34.391, -14.000 -2.868 -31.924, -14.000 -3.044 -34.932, -11.500 -2.000 -32.000, -11.500 -2.868 -31.924, -11.500 -3.044 -34.932, -11.500 -4.071 -34.727, -11.500 -3.710 -31.698, -11.500 -0.290 -31.698, -11.500 -1.132 -31.924, -11.500 0.071 -34.727, -11.500 1.061 -34.391, -11.500 -6.929 -26.162, -11.500 -6.719 -25.348, -11.500 -9.664 -24.706, -11.500 -9.916 -25.841, -11.500 -0.956 -34.932, -11.500 -2.000 -35.000, -14.000 -2.000 -35.000, -14.000 -0.663 -22.182, -14.000 -1.476 -22.028, -14.000 -0.381 -19.166, -14.000 0.613 -19.439, 39.500 -5.214 -30.830, 39.500 -10.485 -35.485, 39.500 -5.830 -30.214, 39.500 -9.482 -36.382, 39.500 -4.500 -31.330, 39.500 -8.384 -37.161, 39.500 -7.207 -37.812, 39.500 -3.710 -31.698, 39.500 -5.963 -38.327, 39.500 -2.868 -31.924, 39.500 -4.670 -38.699, 39.500 -3.344 -38.925, 39.500 -2.000 -32.000, 39.500 -2.000 -39.000, 39.500 -1.159 -31.929, 39.500 -0.689 -38.928, 39.500 0.606 -38.714, -11.500 -5.061 -34.391, -11.500 -6.000 -33.928, -11.500 -6.870 -33.347, -11.500 -7.657 -32.657, -14.000 -8.347 -31.870, -11.500 -8.347 -31.870, -14.000 -8.928 -31.000, -11.500 -8.928 -31.000, -14.000 -9.391 -30.061, -11.500 -9.391 -30.061, -14.000 -9.727 -29.071, -11.500 -9.727 -29.071, -11.500 -9.932 -28.044, -14.000 -9.932 -28.044, -14.000 -10.000 -27.000, -11.500 -7.595 -20.661, -11.500 -8.684 -22.604, -11.500 -7.976 -21.682, -11.500 -7.142 -20.872, -11.500 -6.312 -20.262, -11.500 -5.411 -19.763, -11.500 -3.556 -12.000, -11.500 -1.797 -16.854, -11.500 -4.453 -19.385, -11.500 -3.454 -19.133, -11.500 -2.431 -19.012, -11.500 -1.401 -19.022, -11.500 -0.381 -19.166, -11.500 0.613 -19.439, -14.000 -8.684 -22.604, -14.000 -7.595 -20.661, -14.000 -7.976 -21.682, -14.000 -7.142 -20.872, -14.000 -4.453 -19.385, -14.000 -1.797 -16.854, -14.000 -3.454 -19.133, -14.000 -2.431 -19.012, -14.000 -1.401 -19.022, -14.000 -3.556 -12.000, -14.000 4.700 -22.628, -14.000 4.000 -21.708, -14.000 5.803 -17.777, -14.000 5.260 -23.639, -14.000 5.668 -24.719, -14.000 4.000 -12.000, -14.000 5.145 -12.000, 17.380 -11.788 -5.000, 18.796 -9.365 -5.000, 24.574 -14.000 -5.000, 15.652 -14.000 -5.000, 25.326 -8.862 -5.000, 26.530 -5.361 -5.000, 30.081 -7.249 -5.000, 19.877 -6.775 -5.000, 42.000 1.500 -23.429, 42.000 1.500 -15.522, 42.000 0.802 -22.859, 42.000 0.013 -22.423, 42.000 -0.841 -22.136, 42.000 -6.285 -24.423, 42.000 -6.250 -21.003, 42.000 -12.236 -20.737, 42.000 -12.861 -21.897, 42.000 -2.000 -39.000, 42.000 -3.344 -38.925, 42.000 -4.670 -38.699, 42.000 -5.963 -38.327, 42.000 -7.207 -37.812, 42.000 -8.384 -37.161, 42.000 -9.482 -36.382, 42.000 -10.485 -35.485, 42.000 -11.382 -34.482, 39.500 -11.382 -34.482, 42.000 -12.161 -33.384, 39.500 -12.161 -33.384, 42.000 -12.812 -32.207, 39.500 -12.812 -32.207, 42.000 -13.327 -30.963, 39.500 -13.327 -30.963, 42.000 -13.699 -29.670, 39.500 -13.699 -29.670, 42.000 -13.925 -28.344, 39.500 -13.925 -28.344, 39.500 -14.000 -27.000, 17.126 6.000 -5.000, 20.946 1.500 -5.000, 17.286 3.750 -5.000, 13.626 1.500 -5.000, 13.305 1.671 -5.000, 14.834 -14.865 -5.000, 8.145 -16.463 -5.000, 13.967 -15.682 -5.000, 13.056 -16.448 -5.000, 6.577 -17.826 -5.000, 0.638 -14.000 -5.000, 4.980 -18.336 -5.000, 6.577 -17.826 -12.000, 3.962 -18.582 -12.000, -14.000 12.995 -12.000, -14.000 12.845 -12.000, -14.000 12.845 -8.409, -14.000 14.249 -8.500, -0.000 -11.000 -5.000, 4.934 -6.250 -5.000, 1.226 -3.807 -5.000, 42.000 -2.401 -15.007, 42.000 1.500 -12.000, 42.000 -3.713 -15.123, 42.000 -1.085 -15.035, 42.000 0.221 -15.207, 42.000 -14.000 -12.000, 42.000 -14.000 -27.000, 39.500 -14.000 -12.000, -0.056 -21.000 -12.000, -1.455 -18.944 -12.000, -0.094 -19.972 -12.000, 1.267 -18.958 -12.000, 1.214 -19.979 -12.000, 2.483 -20.853 -12.000, -11.500 -12.000 -12.000, -14.000 -11.000 -12.000, -11.500 -11.000 -12.000, -11.500 -10.000 -12.000, -14.000 -10.000 -12.000, -14.000 -12.000 -12.000, -14.000 -12.845 -12.000, -11.500 -15.124 -12.000, -14.000 -15.652 -12.000, -12.005 -17.230 -12.000, 4.986 -20.400 -12.000, -9.221 -16.612 -12.000, -9.834 -18.555 -12.000, -6.753 -17.759 -12.000, -7.519 -19.608 -12.000, 7.415 -19.647 -12.000, 33.984 -12.000 -12.000, 33.984 -14.000 -12.000, 35.000 -14.000 -12.000, 39.000 -12.000 -12.000, -5.094 -20.373 -12.000, -4.146 -18.542 -12.000, -2.594 -20.839 -12.000, -11.500 -11.000 -5.000, -11.000 1.500 -5.000, -11.000 -14.000 -5.000, -11.500 -10.000 -5.000, -11.500 -4.556 -5.000, -11.500 -3.556 -5.000, -11.662 15.000 -5.000, -9.583 16.406 -5.000, -11.500 15.000 -5.000, -11.500 12.995 -5.000, -11.500 11.995 -5.000, -11.500 4.000 -5.000, -3.708 1.500 -5.000, -3.189 2.415 -5.000, -11.950 17.269 -12.000, -14.000 15.652 -12.000, -14.000 15.652 -5.000, -11.950 17.269 -5.000, 7.415 -19.647 -5.000, 4.676 -20.473 -5.000, 18.975 -0.974 -2.000, 23.664 -3.970 -2.000, 18.994 -0.487 -2.000, 18.944 -1.460 -2.000, 26.518 -0.235 -2.000, 32.265 -2.142 -2.000, -19.170 -8.573 -2.000, -16.919 -8.646 -2.000, -17.928 -6.291 -2.000, -17.784 -11.168 -2.000, 18.809 -2.690 -2.000, 12.836 1.788 -2.000, 18.594 -3.908 -2.000, 18.300 -5.110 -2.000, 13.746 3.500 -2.000, 1.936 3.500 -2.000, 2.789 2.867 -2.000, 3.446 2.032 -2.000, 34.733 1.500 -2.000, 34.776 1.580 -2.000, 34.361 1.800 -2.000, 34.235 -1.723 -2.000, 35.000 1.500 -2.000, 39.000 1.500 -2.000, 42.000 -14.000 -2.000, 42.000 1.500 -2.000, 39.000 -12.000 -2.000, 39.500 1.500 -2.000, 39.500 -14.000 -2.000, 35.000 -14.000 -2.000, 33.984 -12.000 -2.000, 33.984 -14.000 -2.000, 7.415 -19.647 -2.000, 6.577 -17.826 -2.000, 4.986 -20.400 -2.000, 3.962 -18.582 -2.000, 2.483 -20.853 -2.000, 1.267 -18.958 -2.000, 1.214 -19.979 -2.000, -0.056 -21.000 -2.000, -1.455 -18.944 -2.000, -0.094 -19.972 -2.000, -2.594 -20.839 -2.000, -4.146 -18.542 -2.000, -5.094 -20.373 -2.000, -6.753 -17.759 -2.000, -7.519 -19.608 -2.000, -9.834 -18.555 -2.000, -9.221 -16.612 -2.000, -12.005 -17.230 -2.000, -11.500 -15.124 -2.000, -14.000 -15.652 -2.000, -11.662 15.000 -2.000, -11.950 17.269 -2.000, -14.000 15.652 -2.000, -14.000 12.845 -2.000, -11.500 15.000 -2.000, -11.500 5.145 -2.000, -14.000 5.145 -2.000, -14.000 3.500 -2.000, -11.500 3.000 -2.000, -11.500 3.500 -2.000, -14.000 3.000 -2.000, -11.500 -1.292 -2.000, -14.000 -1.292 -2.000, -14.000 -11.000 -2.000, -11.500 -11.000 -2.000, -14.000 -12.000 -2.000, -11.500 -12.000 -2.000, -14.000 -12.845 -2.000, -9.563 16.418 -2.000, 13.722 13.142 -2.000, 16.286 13.257 -2.000, 14.515 15.176 -2.000, 11.851 14.851 -2.000, 12.520 16.860 -2.000, 15.348 11.200 -2.000, 17.805 11.134 -2.000, 9.770 16.296 -2.000, 10.331 18.283 -2.000, 16.702 9.058 -2.000, 7.515 17.451 -2.000, 7.983 19.424 -2.000, 5.127 18.295 -2.000, 5.511 20.264 -2.000, 2.647 18.815 -2.000, 2.954 20.791 -2.000, 0.351 20.997 -2.000, 0.120 19.000 -2.000, -2.257 20.878 -2.000, -2.408 18.847 -2.000, -4.830 20.437 -2.000, -4.894 18.359 -2.000, -7.329 19.680 -2.000, -7.293 17.544 -2.000, -9.714 18.618 -2.000, -8.504 17.518 -2.000, 33.984 -14.000 -5.000, 35.000 -14.000 -5.000, 39.500 -14.000 -5.000, 42.000 -14.000 -5.000, 42.000 1.500 -5.000, 39.500 1.500 -5.000, 39.000 1.500 -5.000, 38.125 1.500 -3.500, 37.250 1.500 -3.500, 35.000 1.500 -5.000, 36.125 1.500 -3.500, 36.314 -11.505 -2.000, 35.633 -5.100 -2.000, 8.890 -16.792 -2.000, 19.864 -12.955 -2.000, 13.003 -13.853 -2.000, 14.731 -12.000 -2.000, 11.045 -15.460 -2.000, 2.079 -18.781 -2.000, -2.462 -15.479 -2.000, 0.638 -12.000 -2.000, -0.191 -15.391 -2.000, -3.432 2.055 -2.000, -2.780 2.876 -2.000, -1.936 3.500 -2.000, -3.848 1.092 -2.000, -4.000 0.055 -2.000, -3.876 -0.987 -2.000, -0.000 -9.000 -2.000, -2.032 -3.446 -2.000, 3.722 -4.250 -2.000, 1.013 -3.870 -2.000, 3.863 -1.040 -2.000, 3.460 -2.008 -2.000, 2.819 -2.838 -2.000, 1.984 -3.473 -2.000, -0.027 -4.000 -2.000, -1.066 -3.855 -2.000, -3.487 -1.960 -2.000, 4.000 0.000 -2.000, 3.859 1.053 -2.000, -2.857 -2.799 -2.000, 2.520 11.250 -2.000, -1.000 3.873 -2.000, 0.000 4.000 -2.000, 1.000 3.873 -2.000, 17.963 10.877 -2.000, 18.118 10.619 -2.000, 18.268 10.357 -2.000, 17.551 7.277 -2.000, 23.109 4.728 -2.000, 31.165 3.500 -2.000, 18.675 3.500 -2.000, 18.211 5.418 -2.000, 1.850 -20.918 -5.000, -1.010 -20.976 -5.000, -3.851 -20.644 -5.000, -6.621 -19.929 -5.000, -9.268 -18.844 -5.000, -11.743 -17.410 -5.000, -14.000 -15.652 -5.000, -15.903 -13.715 -5.000, -16.050 -13.543 -2.000, -17.539 -11.548 -5.000, -18.883 -9.189 -5.000, -19.911 -6.675 -5.000, -20.180 -5.811 -2.000, -20.606 -4.050 -5.000, -20.794 -2.934 -2.000, -20.956 -1.358 -5.000, -21.000 0.000 -2.000, -20.956 1.358 -5.000, -20.794 2.934 -2.000, -20.606 4.050 -5.000, -20.180 5.811 -2.000, -19.911 6.675 -5.000, -19.170 8.573 -2.000, -18.883 9.189 -5.000, -17.784 11.168 -2.000, -17.539 11.548 -5.000, -16.050 13.543 -2.000, -15.903 13.715 -5.000, -9.714 18.618 -5.000, -7.329 19.680 -5.000, -4.830 20.437 -5.000, -2.257 20.878 -5.000, 0.351 20.997 -5.000, 2.954 20.791 -5.000, 5.511 20.264 -5.000, 7.983 19.424 -5.000, 10.331 18.283 -5.000, 12.520 16.860 -5.000, 14.515 15.176 -5.000, 16.286 13.257 -5.000, 17.805 11.134 -5.000, 17.963 10.877 -5.000, 18.118 10.619 -5.000, 18.268 10.357 -5.000, -15.601 -10.844 -2.000, -15.601 10.844 -2.000, -16.919 8.646 -2.000, -19.054 7.192 -2.000, -17.928 6.291 -2.000, -19.703 4.372 -2.000, -18.612 3.821 -2.000, -19.978 1.467 -2.000, -18.957 1.281 -2.000, -19.978 -1.467 -2.000, -18.957 -1.281 -2.000, -19.978 0.000 -2.000, -18.612 -3.821 -2.000, -19.703 -4.372 -2.000, -19.054 -7.192 -2.000, -16.478 0.000 -2.000, -17.718 0.000 -2.000, -16.306 1.910 -2.000, 39.500 1.500 -12.000, 34.733 1.500 -5.000, 39.000 1.500 -12.000, 37.000 -5.250 -5.000, 35.000 -7.321 -5.000, 39.000 -12.000 -5.000, 35.994 -12.000 -5.000, 34.776 1.580 -5.000, 26.522 5.969 -3.500, 18.855 2.341 -2.000, 18.964 1.173 -2.000, 19.000 -0.000 -2.000, 29.612 -7.132 -2.000, 32.963 -6.614 -2.000, 25.523 -6.862 -2.000, 17.430 -7.563 -2.000, 16.233 -9.874 -2.000, 7.684 -15.391 -2.000, 7.733 -16.608 -2.000, 3.874 -0.997 -5.000, 4.000 0.000 -5.000, 3.503 -1.931 -5.000, 2.912 -2.743 -5.000, 2.136 -3.382 -5.000, 0.238 -3.993 -5.000, -0.764 -3.926 -5.000, -1.718 -3.612 -5.000, -2.564 -3.070 -5.000, -3.248 -2.334 -5.000, -3.728 -1.451 -5.000, -3.972 -0.476 -5.000, -3.965 0.529 -5.000, -2.450 3.162 -5.000, -1.541 3.691 -5.000, -0.526 3.965 -5.000, 0.526 3.965 -5.000, 1.541 3.691 -5.000, 2.450 3.162 -5.000, 3.189 2.415 -5.000, 3.708 1.500 -5.000, 3.869 1.014 -5.000, 3.967 0.511 -5.000, 15.918 1.020 -2.000, 20.982 -3.285 -2.000, 9.150 -8.555 -2.000, 16.211 6.279 -2.000, 17.211 6.348 -2.000, 16.211 4.890 -2.000, -14.000 -10.000 -5.000, -14.000 -4.556 -5.000, -14.000 -11.000 -5.000, -15.770 -12.358 -5.000, -14.000 15.000 -5.000, -14.000 -12.845 -5.000, -14.000 -15.000 -5.000, -14.000 -15.500 -5.000, -14.000 12.995 -5.000, -17.478 0.000 -5.000, -14.000 12.845 -5.000, -17.303 2.025 -5.000, -14.000 3.000 -5.000, -15.739 -0.278 -5.000, -14.000 -3.556 -5.000, -17.303 -2.025 -5.000, -14.000 11.995 -5.000, -15.770 12.632 -5.000, -14.000 4.000 -5.000, 12.520 16.860 -12.000, 10.331 18.283 -12.000, 14.515 15.176 -12.000, -9.714 18.618 -12.000, 16.286 13.257 -12.000, -7.329 19.680 -12.000, 17.805 11.134 -12.000, -4.830 20.437 -12.000, -2.257 20.878 -12.000, 0.351 20.997 -12.000, 2.954 20.791 -12.000, 5.511 20.264 -12.000, 7.983 19.424 -12.000, -7.338 17.526 -5.000, -4.964 18.340 -5.000, 18.000 10.500 -5.000, -2.504 18.834 -5.000, 0.000 19.000 -5.000, 16.702 9.058 -5.000, 2.543 18.829 -5.000, 5.040 18.319 -5.000, 7.446 17.480 -5.000, 9.718 16.326 -5.000, 13.701 13.164 -5.000, 15.339 11.212 -5.000, -11.500 3.000 -5.000, 11.816 14.879 -5.000, -11.500 -15.000 -5.000, -11.500 -15.124 -5.000, -9.148 -16.653 -5.000, -6.596 -17.818 -5.000, -3.901 -18.595 -5.000, -1.120 -18.967 -5.000, 1.685 -18.925 -5.000, 42.000 -13.928 -25.685, 42.000 -13.712 -24.386, 42.000 -13.355 -23.118, 42.000 -11.488 -19.653, 42.000 -10.626 -18.658, 42.000 -9.660 -17.763, 42.000 -8.601 -16.979, 42.000 -7.463 -16.316, 42.000 -6.260 -15.781, 42.000 -5.005 -15.382, 39.500 1.500 -15.522, 34.235 -3.723 -5.000, 31.666 -4.269 -5.000, 35.000 -12.000 -5.000, 36.492 -12.000 -10.250, 36.492 -12.000 -8.500, 33.984 -12.000 -5.000, 35.238 -12.000 -8.500, 37.746 -12.000 -8.500, 24.981 3.071 -5.000, 19.596 7.549 -5.000, 20.496 4.574 -5.000, 27.965 -4.637 -2.000, 20.644 -3.848 -5.000, 20.868 -2.351 -5.000, 20.314 -5.325 -5.000, -14.000 -14.249 -8.500, -14.000 12.845 -6.824, -14.000 13.923 -6.750, -14.000 13.452 -8.126, -14.000 -11.923 -8.500, -14.000 -6.146 -8.500, -14.000 -1.292 -12.000, -14.000 -4.556 -12.000, -14.000 -8.073 -8.500, -14.000 -0.778 -8.500, -14.000 3.000 -12.000, -14.000 1.111 -8.500, -14.000 3.500 -12.000, -14.000 7.997 -6.750, -14.000 7.923 -8.500, -14.000 5.961 -8.500, -11.662 15.000 -12.000, -11.639 12.995 -12.000, 0.120 19.000 -12.000, 2.647 18.815 -12.000, -2.408 18.847 -12.000, -4.894 18.359 -12.000, 13.722 13.142 -12.000, -7.293 17.544 -12.000, 11.851 14.851 -12.000, 15.348 11.200 -12.000, -9.563 16.418 -12.000, -8.504 17.518 -12.000, 9.770 16.296 -12.000, 16.702 9.058 -12.000, 7.515 17.451 -12.000, 5.127 18.295 -12.000, -11.500 15.000 -12.000, -11.500 12.995 -12.000, -11.500 13.452 -8.126, -11.500 11.995 -12.000, -11.500 5.145 -12.000, -11.500 12.845 -12.000, -11.500 3.500 -12.000, -11.500 3.000 -12.000, -11.500 4.000 -12.000, -11.500 8.226 -8.500, -11.500 7.997 -6.750, -11.500 6.113 -8.500, -11.500 -0.778 -8.500, -11.500 1.111 -8.500, -11.500 -1.292 -12.000, -11.500 -6.146 -8.500, -11.500 -4.556 -12.000, -11.500 -8.073 -8.500, -11.500 -13.062 -8.500, -11.500 -12.031 -8.500, -11.500 -13.000 -6.750, -11.500 -14.093 -8.500, 3.345 -18.703 -5.000, 39.500 0.221 -15.207, 39.500 -2.401 -15.007, 39.500 -3.713 -15.123, 39.500 -1.085 -15.035, 39.500 -13.928 -25.685, 39.500 -13.712 -24.386, 39.500 -13.355 -23.118, 39.500 -12.861 -21.897, 39.500 -12.236 -20.737, 39.500 -11.488 -19.653, 39.500 -10.626 -18.658, 39.500 -9.660 -17.763, 39.500 -8.601 -16.979, 39.500 -7.463 -16.316, 39.500 -6.260 -15.781, 39.500 -5.005 -15.382, 20.280 -14.913 -8.500, -11.500 -10.000 -27.000, 42.000 -2.633 -22.040, 42.000 -6.678 -25.234, 42.000 -1.733 -22.007, 42.000 -6.919 -26.103, 42.000 -7.000 -27.000, 42.000 -5.753 -23.696, 42.000 -5.099 -23.076, 42.000 -4.344 -22.583, 42.000 -3.513 -22.234, 39.500 2.733 -15.973, 42.000 2.733 -15.973, 42.000 3.909 -16.556, 39.500 3.909 -16.556, 39.500 5.014 -17.264, 42.000 5.014 -17.264, 42.000 6.036 -18.088, 39.500 6.036 -18.088, 39.500 6.961 -19.019, 42.000 6.961 -19.019, 42.000 7.779 -20.046, 39.500 7.779 -20.046, 39.500 8.481 -21.156, 42.000 8.481 -21.156, 39.500 9.056 -22.335, 42.000 9.056 -22.335, 39.500 9.500 -23.571, 42.000 9.500 -23.571, 39.500 9.805 -24.847, 42.000 9.805 -24.847, 39.500 9.970 -26.150, 42.000 9.970 -26.150, 39.500 9.991 -27.462, 42.000 9.991 -27.462, 39.500 9.869 -28.769, 42.000 9.869 -28.769, 39.500 9.605 -30.055, 42.000 9.605 -30.055, 39.500 9.201 -31.304, 42.000 9.201 -31.304, 39.500 8.664 -32.502, 42.000 8.664 -32.502, 39.500 7.999 -33.634, 42.000 7.999 -33.634, 39.500 7.215 -34.687, 42.000 7.215 -34.687, 42.000 6.320 -35.647, 39.500 6.320 -35.647, 39.500 5.326 -36.504, 42.000 5.326 -36.504, 39.500 4.244 -37.248, 42.000 4.244 -37.248, 39.500 3.087 -37.868, 42.000 3.087 -37.868, 39.500 1.870 -38.359, 42.000 1.870 -38.359, 42.000 0.606 -38.714, 42.000 -0.689 -38.928, 27.801 -1.930 -5.000, 20.976 1.000 -5.000, 20.994 0.500 -5.000, 21.000 -0.000 -5.000, 20.985 -0.785 -5.000, 20.941 -1.569 -5.000, 9.939 -10.388 -5.000, 23.203 -4.563 -5.000, -14.000 11.995 -12.000, -14.000 5.917 -25.848, -14.000 6.000 -27.000, -14.000 3.270 -20.981, -14.000 2.453 -20.354, -14.000 1.562 -19.837, -14.000 -5.411 -19.763, -14.000 -6.312 -20.262, -14.000 -9.916 -25.841, -14.000 -9.664 -24.706, -14.000 -9.250 -23.619, -12.750 9.497 -19.500, -11.500 6.000 -27.000, -11.500 5.917 -25.848, -11.500 5.668 -24.719, -11.500 5.803 -17.777, -11.500 4.000 -21.708, -11.500 4.700 -22.628, -11.500 5.260 -23.639, -11.500 3.270 -20.981, -11.500 2.453 -20.354, -11.500 1.562 -19.837, -11.500 -9.250 -23.619, 18.622 -14.317 -5.000, 39.500 -6.250 -21.003, 39.500 -2.633 -22.040, 39.500 -5.099 -23.076, 39.500 -5.753 -23.696, 39.500 -4.344 -22.583, 39.500 -3.513 -22.234, 39.500 -6.285 -24.423, 39.500 -0.841 -22.136, 39.500 0.013 -22.423, 39.500 1.500 -23.429, 39.500 0.802 -22.859, 39.500 -1.733 -22.007, 39.500 -6.678 -25.234, 39.500 -6.919 -26.103, 39.500 -7.000 -27.000, 42.000 2.051 -24.069, 42.000 2.486 -24.792, 42.000 2.794 -25.578, 42.000 2.964 -26.405, 42.000 2.994 -27.248, 42.000 2.881 -28.085, 42.000 2.629 -28.890, 42.000 2.245 -29.642, 42.000 1.740 -30.318, 42.000 1.129 -30.900, 42.000 0.428 -31.371, 42.000 -0.342 -31.717, 42.000 -1.159 -31.929, 42.000 -2.000 -32.000, 42.000 -6.924 -27.868, 42.000 -2.868 -31.924, 42.000 -6.698 -28.710, 42.000 -3.710 -31.698, 42.000 -6.330 -29.500, 42.000 -4.500 -31.330, 42.000 -5.830 -30.214, 42.000 -5.214 -30.830, 39.500 2.051 -24.069, 39.500 2.486 -24.792, 39.500 2.794 -25.578, 39.500 2.964 -26.405, 39.500 2.994 -27.248, 39.500 2.881 -28.085, 39.500 2.629 -28.890, 39.500 2.245 -29.642, 39.500 1.740 -30.318, 39.500 1.129 -30.900, 39.500 0.428 -31.371, 39.500 -0.342 -31.717, 39.500 -6.924 -27.868, 39.500 -6.698 -28.710, 39.500 -6.330 -29.500, 29.098 -5.759 -5.000, -14.000 4.000 -26.067, -14.000 5.000 -24.354, -14.000 -2.303 -22.009, -14.000 -3.122 -22.127, -14.000 -3.910 -22.379, -14.000 1.472 -23.402, -14.000 2.018 -24.024, -14.000 0.831 -22.879, -14.000 2.454 -24.727, -14.000 0.113 -22.468, -14.000 2.767 -25.493, -14.000 -4.657 -22.765, -14.000 -5.330 -23.270, -14.000 -6.929 -26.162, -14.000 -6.719 -25.348, -14.000 -7.000 -27.000, -14.000 -6.955 -23.936, -14.000 -6.375 -24.580, -14.000 -5.908 -23.881, -14.000 -6.431 -24.230, -14.000 5.932 -28.044, -11.500 5.932 -28.044, -11.500 5.727 -29.071, -14.000 5.391 -30.061, -11.500 5.391 -30.061, -14.000 4.928 -31.000, -11.500 4.928 -31.000, -14.000 4.347 -31.870, -11.500 4.347 -31.870, -11.500 3.657 -32.657, -14.000 3.657 -32.657, -14.000 2.870 -33.347, -11.500 2.870 -33.347, -11.500 2.000 -33.928, -14.000 2.000 -33.928, -11.500 4.000 -26.067, -11.500 5.000 -24.354, -11.500 -2.303 -22.009, -11.500 -1.476 -22.028, -11.500 -3.122 -22.127, -11.500 -3.910 -22.379, -11.500 1.472 -23.402, -11.500 2.018 -24.024, -11.500 0.831 -22.879, -11.500 2.454 -24.727, -11.500 0.113 -22.468, -11.500 2.767 -25.493, -11.500 -0.663 -22.182, -11.500 -4.657 -22.765, -11.500 -5.330 -23.270, -11.500 -7.000 -27.000, -11.500 -6.955 -23.936, -11.500 -6.375 -24.580, -11.500 -5.908 -23.881, -11.500 -6.431 -24.230, -11.500 4.000 -27.000, -11.500 -6.924 -27.868, -11.500 -6.698 -28.710, -11.500 -6.330 -29.500, -11.500 2.924 -27.868, -11.500 3.000 -27.000, -11.500 2.698 -28.710, -11.500 -4.500 -31.330, -11.500 2.330 -29.500, -11.500 -5.830 -30.214, -11.500 1.830 -30.214, -11.500 -5.214 -30.830, -11.500 1.214 -30.830, -11.500 0.500 -31.330, -14.000 -6.924 -27.868, -14.000 -6.698 -28.710, -14.000 -6.330 -29.500, -14.000 2.698 -28.710, -14.000 2.330 -29.500, -14.000 1.830 -30.214, -14.000 1.214 -30.830, -14.000 0.500 -31.330, -11.500 2.896 -25.986, -14.000 2.896 -25.986, -14.000 2.974 -26.490, -11.500 2.974 -26.490, -14.000 3.384 -26.246, -11.500 3.384 -26.246 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 3, 1, 0, -1, 4, 5, 6, -1, 4, 7, 5, -1, 8, 7, 4, -1, 8, 9, 7, -1, 10, 11, 12, -1, 10, 13, 11, -1, 14, 8, 15, -1, 16, 14, 15, -1, 17, 3, 0, -1, 18, 3, 17, -1, 19, 16, 15, -1, 20, 16, 19, -1, 21, 22, 23, -1, 24, 23, 22, -1, 22, 25, 24, -1, 26, 27, 28, -1, 29, 26, 28, -1, 30, 31, 32, -1, 30, 32, 33, -1, 3, 28, 34, -1, 1, 3, 34, -1, 1, 34, 35, -1, 36, 1, 35, -1, 37, 38, 39, -1, 40, 37, 39, -1, 41, 42, 43, -1, 41, 44, 42, -1, 45, 46, 41, -1, 47, 46, 45, -1, 48, 47, 45, -1, 49, 47, 48, -1, 50, 51, 48, -1, 52, 51, 50, -1, 53, 52, 50, -1, 54, 52, 53, -1, 55, 56, 53, -1, 57, 56, 55, -1, 20, 35, 23, -1, 20, 36, 35, -1, 23, 16, 20, -1, 23, 24, 16, -1, 24, 14, 16, -1, 24, 58, 14, -1, 58, 9, 14, -1, 58, 59, 9, -1, 59, 7, 9, -1, 59, 60, 7, -1, 5, 7, 60, -1, 5, 60, 61, -1, 62, 5, 61, -1, 62, 61, 63, -1, 63, 64, 62, -1, 63, 65, 64, -1, 65, 66, 64, -1, 67, 68, 66, -1, 66, 65, 67, -1, 67, 69, 68, -1, 69, 70, 71, -1, 69, 71, 68, -1, 70, 72, 71, -1, 73, 74, 75, -1, 76, 73, 75, -1, 76, 77, 73, -1, 78, 73, 77, -1, 78, 79, 73, -1, 80, 81, 82, -1, 83, 80, 82, -1, 84, 80, 83, -1, 85, 80, 84, -1, 86, 80, 85, -1, 81, 80, 79, -1, 87, 88, 89, -1, 90, 89, 88, -1, 91, 92, 93, -1, 94, 93, 92, -1, 95, 94, 92, -1, 39, 95, 92, -1, 40, 39, 92, -1, 92, 91, 96, -1, 97, 98, 99, -1, 100, 97, 99, -1, 101, 100, 99, -1, 99, 98, 102, -1, 99, 102, 103, -1, 104, 105, 106, -1, 107, 104, 106, -1, 106, 105, 108, -1, 108, 109, 110, -1, 108, 111, 109, -1, 105, 111, 108, -1, 106, 108, 110, -1, 112, 113, 114, -1, 114, 113, 115, -1, 115, 113, 116, -1, 117, 118, 119, -1, 117, 119, 120, -1, 52, 121, 122, -1, 52, 54, 121, -1, 122, 51, 52, -1, 51, 122, 123, -1, 123, 49, 51, -1, 49, 123, 124, -1, 124, 47, 49, -1, 47, 124, 125, -1, 125, 46, 47, -1, 125, 126, 46, -1, 126, 44, 46, -1, 126, 127, 44, -1, 127, 128, 44, -1, 42, 44, 128, -1, 42, 128, 129, -1, 129, 130, 42, -1, 130, 129, 131, -1, 131, 132, 130, -1, 131, 133, 132, -1, 133, 134, 132, -1, 134, 133, 135, -1, 135, 136, 134, -1, 135, 137, 136, -1, 137, 138, 136, -1, 137, 139, 138, -1, 139, 140, 138, -1, 139, 141, 140, -1, 142, 143, 144, -1, 143, 145, 144, -1, 145, 142, 144, -1, 145, 146, 142, -1, 147, 148, 107, -1, 149, 148, 147, -1, 150, 148, 149, -1, 151, 148, 150, -1, 107, 148, 152, -1, 151, 152, 148, -1, 153, 151, 154, -1, 154, 155, 153, -1, 156, 157, 158, -1, 159, 156, 158, -1, 160, 161, 111, -1, 160, 162, 161, -1, 163, 164, 165, -1, 166, 164, 163, -1, 167, 164, 166, -1, 113, 164, 167, -1, 165, 164, 168, -1, 141, 169, 168, -1, 170, 141, 168, -1, 171, 172, 173, -1, 172, 174, 173, -1, 174, 171, 173, -1, 171, 174, 175, -1, 174, 176, 175, -1, 176, 171, 175, -1, 177, 178, 179, -1, 179, 178, 180, -1, 178, 181, 180, -1, 182, 178, 177, -1, 183, 182, 177, -1, 184, 183, 177, -1, 185, 183, 184, -1, 186, 185, 184, -1, 176, 174, 155, -1, 187, 176, 155, -1, 186, 184, 188, -1, 189, 186, 188, -1, 189, 188, 190, -1, 191, 189, 190, -1, 187, 155, 154, -1, 192, 187, 154, -1, 192, 154, 193, -1, 194, 192, 193, -1, 195, 193, 196, -1, 170, 195, 196, -1, 197, 190, 198, -1, 199, 197, 198, -1, 200, 201, 202, -1, 203, 201, 200, -1, 204, 201, 203, -1, 205, 201, 204, -1, 206, 207, 208, -1, 209, 208, 207, -1, 207, 210, 209, -1, 201, 211, 212, -1, 212, 211, 213, -1, 214, 215, 216, -1, 214, 216, 217, -1, 218, 187, 192, -1, 218, 219, 187, -1, 220, 221, 222, -1, 223, 221, 220, -1, 221, 224, 222, -1, 225, 224, 221, -1, 226, 227, 228, -1, 227, 226, 229, -1, 230, 223, 231, -1, 232, 230, 231, -1, 233, 232, 231, -1, 234, 235, 236, -1, 237, 234, 236, -1, 231, 234, 237, -1, 238, 239, 240, -1, 241, 238, 240, -1, 225, 241, 240, -1, 241, 242, 238, -1, 241, 243, 242, -1, 244, 245, 246, -1, 245, 247, 246, -1, 247, 243, 246, -1, 248, 244, 246, -1, 248, 246, 249, -1, 249, 246, 250, -1, 251, 249, 250, -1, 251, 250, 252, -1, 250, 253, 252, -1, 252, 253, 254, -1, 254, 253, 255, -1, 256, 254, 255, -1, 256, 255, 257, -1, 256, 257, 258, -1, 257, 259, 258, -1, 259, 256, 258, -1, 257, 260, 261, -1, 260, 259, 261, -1, 259, 257, 261, -1, 262, 259, 260, -1, 262, 260, 263, -1, 264, 262, 263, -1, 264, 263, 265, -1, 264, 265, 266, -1, 266, 265, 267, -1, 265, 268, 267, -1, 267, 268, 269, -1, 269, 268, 270, -1, 269, 270, 271, -1, 272, 273, 274, -1, 275, 272, 274, -1, 276, 272, 275, -1, 276, 275, 277, -1, 277, 275, 278, -1, 277, 278, 279, -1, 280, 281, 279, -1, 281, 277, 279, -1, 282, 280, 279, -1, 280, 282, 283, -1, 283, 282, 284, -1, 283, 284, 285, -1, 286, 283, 285, -1, 287, 286, 285, -1, 288, 286, 287, -1, 289, 288, 287, -1, 270, 288, 289, -1, 271, 270, 289, -1, 272, 290, 273, -1, 291, 292, 293, -1, 291, 293, 294, -1, 294, 293, 295, -1, 296, 292, 291, -1, 296, 297, 292, -1, 294, 295, 298, -1, 295, 299, 298, -1, 297, 296, 300, -1, 298, 299, 301, -1, 299, 302, 301, -1, 301, 302, 303, -1, 303, 302, 304, -1, 303, 304, 305, -1, 305, 304, 306, -1, 305, 306, 307, -1, 308, 305, 307, -1, 308, 307, 309, -1, 310, 308, 309, -1, 310, 309, 311, -1, 312, 310, 311, -1, 312, 311, 313, -1, 312, 313, 314, -1, 314, 313, 315, -1, 314, 315, 316, -1, 315, 290, 316, -1, 290, 314, 316, -1, 290, 315, 273, -1, 251, 317, 249, -1, 317, 318, 249, -1, 318, 248, 249, -1, 318, 319, 248, -1, 248, 319, 244, -1, 319, 320, 244, -1, 321, 245, 244, -1, 321, 244, 320, -1, 247, 245, 322, -1, 245, 321, 322, -1, 243, 247, 323, -1, 247, 322, 323, -1, 324, 243, 323, -1, 324, 323, 325, -1, 324, 325, 243, -1, 243, 325, 242, -1, 326, 242, 327, -1, 242, 325, 327, -1, 325, 326, 327, -1, 325, 323, 326, -1, 246, 328, 250, -1, 243, 329, 246, -1, 243, 241, 329, -1, 329, 241, 328, -1, 246, 329, 328, -1, 330, 253, 250, -1, 250, 331, 330, -1, 332, 331, 333, -1, 331, 332, 334, -1, 331, 334, 330, -1, 331, 250, 333, -1, 270, 268, 288, -1, 255, 335, 257, -1, 255, 253, 335, -1, 257, 336, 260, -1, 260, 336, 263, -1, 263, 336, 265, -1, 265, 336, 268, -1, 268, 336, 288, -1, 335, 337, 338, -1, 337, 336, 338, -1, 336, 335, 338, -1, 336, 337, 288, -1, 257, 335, 336, -1, 280, 339, 281, -1, 339, 340, 281, -1, 341, 281, 340, -1, 342, 339, 280, -1, 343, 280, 283, -1, 343, 342, 280, -1, 344, 343, 283, -1, 345, 286, 288, -1, 345, 346, 286, -1, 345, 233, 347, -1, 345, 347, 348, -1, 347, 349, 350, -1, 351, 347, 350, -1, 347, 351, 352, -1, 347, 352, 348, -1, 231, 349, 347, -1, 233, 231, 347, -1, 348, 353, 345, -1, 353, 354, 345, -1, 354, 346, 345, -1, 355, 344, 283, -1, 349, 231, 356, -1, 356, 231, 357, -1, 357, 231, 237, -1, 358, 283, 286, -1, 358, 355, 283, -1, 337, 345, 288, -1, 346, 358, 286, -1, 276, 290, 272, -1, 234, 296, 291, -1, 234, 300, 296, -1, 234, 359, 235, -1, 359, 341, 360, -1, 361, 359, 360, -1, 362, 359, 361, -1, 235, 359, 362, -1, 359, 312, 314, -1, 359, 310, 312, -1, 308, 310, 359, -1, 305, 308, 359, -1, 303, 305, 359, -1, 277, 290, 276, -1, 359, 301, 303, -1, 359, 298, 301, -1, 294, 298, 359, -1, 291, 294, 359, -1, 359, 314, 341, -1, 234, 291, 359, -1, 341, 277, 281, -1, 314, 290, 341, -1, 277, 341, 290, -1, 300, 363, 297, -1, 300, 364, 363, -1, 300, 365, 364, -1, 366, 365, 300, -1, 366, 367, 365, -1, 367, 368, 365, -1, 367, 369, 368, -1, 370, 367, 366, -1, 367, 370, 369, -1, 218, 252, 254, -1, 219, 218, 254, -1, 219, 254, 256, -1, 256, 371, 219, -1, 256, 259, 371, -1, 259, 372, 371, -1, 372, 259, 262, -1, 373, 372, 262, -1, 262, 264, 373, -1, 264, 374, 373, -1, 374, 264, 266, -1, 375, 374, 266, -1, 375, 266, 267, -1, 267, 376, 375, -1, 267, 269, 376, -1, 269, 271, 377, -1, 269, 377, 376, -1, 271, 378, 377, -1, 378, 271, 379, -1, 380, 378, 379, -1, 380, 379, 229, -1, 229, 381, 380, -1, 229, 226, 381, -1, 226, 382, 381, -1, 382, 226, 383, -1, 384, 382, 383, -1, 384, 383, 385, -1, 386, 384, 385, -1, 386, 385, 387, -1, 388, 386, 387, -1, 388, 387, 389, -1, 389, 390, 388, -1, 390, 389, 391, -1, 392, 390, 391, -1, 391, 393, 392, -1, 394, 392, 393, -1, 393, 395, 394, -1, 396, 394, 395, -1, 396, 395, 397, -1, 398, 396, 397, -1, 397, 274, 398, -1, 274, 216, 398, -1, 216, 274, 273, -1, 273, 217, 216, -1, 273, 315, 217, -1, 315, 399, 217, -1, 399, 315, 313, -1, 313, 400, 399, -1, 313, 311, 400, -1, 401, 400, 311, -1, 311, 309, 401, -1, 402, 401, 309, -1, 309, 307, 402, -1, 403, 402, 307, -1, 307, 306, 403, -1, 306, 404, 403, -1, 306, 304, 404, -1, 405, 404, 304, -1, 304, 302, 405, -1, 406, 405, 302, -1, 302, 299, 406, -1, 407, 406, 299, -1, 407, 299, 295, -1, 295, 408, 407, -1, 408, 295, 293, -1, 409, 408, 293, -1, 409, 293, 292, -1, 410, 409, 292, -1, 292, 297, 410, -1, 411, 410, 297, -1, 297, 363, 411, -1, 412, 411, 363, -1, 363, 364, 412, -1, 364, 413, 412, -1, 364, 365, 413, -1, 414, 413, 365, -1, 229, 415, 227, -1, 379, 415, 229, -1, 397, 275, 274, -1, 289, 415, 379, -1, 395, 416, 397, -1, 271, 289, 379, -1, 275, 397, 416, -1, 417, 395, 393, -1, 417, 416, 395, -1, 393, 391, 418, -1, 391, 419, 418, -1, 419, 393, 418, -1, 419, 417, 393, -1, 391, 389, 420, -1, 389, 421, 420, -1, 421, 391, 420, -1, 421, 419, 391, -1, 389, 387, 422, -1, 387, 423, 422, -1, 423, 389, 422, -1, 423, 421, 389, -1, 387, 385, 424, -1, 385, 425, 424, -1, 425, 387, 424, -1, 423, 387, 426, -1, 387, 425, 426, -1, 425, 423, 426, -1, 427, 425, 385, -1, 427, 385, 428, -1, 385, 383, 428, -1, 383, 427, 428, -1, 383, 228, 427, -1, 228, 383, 429, -1, 383, 226, 429, -1, 226, 228, 429, -1, 421, 278, 419, -1, 419, 278, 417, -1, 417, 278, 416, -1, 416, 278, 275, -1, 279, 278, 421, -1, 282, 279, 421, -1, 228, 284, 427, -1, 228, 227, 284, -1, 415, 285, 227, -1, 285, 284, 227, -1, 287, 285, 415, -1, 289, 287, 415, -1, 430, 425, 427, -1, 423, 425, 431, -1, 425, 430, 431, -1, 430, 423, 431, -1, 430, 421, 423, -1, 430, 284, 282, -1, 282, 421, 432, -1, 421, 430, 432, -1, 430, 282, 432, -1, 427, 284, 430, -1, 218, 251, 252, -1, 218, 317, 251, -1, 317, 195, 318, -1, 194, 195, 317, -1, 319, 318, 195, -1, 319, 195, 170, -1, 170, 168, 319, -1, 319, 168, 320, -1, 321, 320, 168, -1, 164, 321, 168, -1, 164, 322, 321, -1, 322, 164, 433, -1, 238, 242, 326, -1, 434, 238, 326, -1, 323, 322, 433, -1, 433, 435, 323, -1, 436, 326, 323, -1, 436, 437, 326, -1, 438, 436, 323, -1, 438, 437, 436, -1, 438, 439, 437, -1, 440, 239, 238, -1, 434, 440, 238, -1, 240, 239, 440, -1, 240, 440, 368, -1, 368, 441, 365, -1, 365, 441, 414, -1, 441, 440, 414, -1, 368, 440, 441, -1, 225, 240, 368, -1, 224, 368, 369, -1, 442, 224, 369, -1, 443, 224, 442, -1, 444, 224, 443, -1, 222, 224, 444, -1, 225, 368, 224, -1, 241, 225, 445, -1, 241, 445, 446, -1, 445, 328, 446, -1, 328, 241, 446, -1, 328, 445, 250, -1, 250, 447, 333, -1, 447, 445, 221, -1, 447, 221, 233, -1, 448, 447, 233, -1, 449, 447, 448, -1, 447, 449, 333, -1, 250, 445, 447, -1, 450, 337, 335, -1, 450, 333, 337, -1, 253, 450, 335, -1, 450, 332, 333, -1, 450, 334, 332, -1, 330, 334, 450, -1, 253, 330, 451, -1, 330, 450, 451, -1, 450, 253, 451, -1, 349, 356, 452, -1, 453, 452, 356, -1, 454, 350, 349, -1, 349, 452, 454, -1, 454, 455, 350, -1, 455, 351, 350, -1, 351, 455, 456, -1, 352, 351, 456, -1, 456, 162, 352, -1, 348, 352, 162, -1, 162, 457, 348, -1, 353, 348, 457, -1, 353, 457, 458, -1, 354, 353, 458, -1, 354, 458, 459, -1, 459, 346, 354, -1, 459, 460, 346, -1, 358, 346, 460, -1, 460, 461, 358, -1, 461, 355, 358, -1, 461, 462, 355, -1, 462, 344, 355, -1, 462, 463, 344, -1, 463, 343, 344, -1, 343, 463, 464, -1, 342, 343, 464, -1, 464, 212, 342, -1, 212, 339, 342, -1, 339, 212, 213, -1, 340, 339, 213, -1, 340, 213, 465, -1, 465, 341, 340, -1, 465, 466, 341, -1, 360, 341, 466, -1, 360, 466, 467, -1, 361, 360, 467, -1, 467, 468, 361, -1, 468, 362, 361, -1, 468, 469, 362, -1, 235, 362, 469, -1, 235, 469, 470, -1, 236, 235, 470, -1, 236, 470, 471, -1, 237, 236, 471, -1, 237, 471, 472, -1, 357, 237, 472, -1, 472, 473, 357, -1, 357, 473, 474, -1, 474, 356, 357, -1, 474, 453, 356, -1, 475, 234, 231, -1, 369, 234, 475, -1, 442, 369, 475, -1, 443, 442, 475, -1, 444, 443, 475, -1, 223, 475, 231, -1, 222, 444, 475, -1, 220, 222, 475, -1, 223, 220, 475, -1, 476, 221, 223, -1, 230, 476, 223, -1, 476, 230, 232, -1, 233, 476, 232, -1, 221, 476, 233, -1, 333, 477, 337, -1, 477, 345, 337, -1, 448, 233, 477, -1, 449, 448, 477, -1, 333, 449, 477, -1, 477, 233, 345, -1, 234, 478, 300, -1, 478, 366, 300, -1, 370, 366, 479, -1, 366, 478, 479, -1, 478, 370, 479, -1, 370, 478, 369, -1, 234, 369, 480, -1, 369, 478, 480, -1, 478, 234, 480, -1, 176, 219, 371, -1, 219, 176, 187, -1, 371, 372, 171, -1, 171, 176, 371, -1, 372, 373, 199, -1, 372, 199, 171, -1, 197, 199, 373, -1, 373, 374, 197, -1, 191, 197, 374, -1, 191, 374, 375, -1, 375, 189, 191, -1, 375, 376, 189, -1, 186, 189, 376, -1, 376, 377, 186, -1, 185, 186, 377, -1, 380, 381, 481, -1, 481, 381, 482, -1, 483, 380, 481, -1, 378, 380, 484, -1, 380, 483, 484, -1, 483, 378, 484, -1, 485, 398, 216, -1, 486, 378, 483, -1, 487, 378, 486, -1, 488, 378, 487, -1, 489, 398, 485, -1, 377, 378, 488, -1, 384, 386, 490, -1, 386, 388, 490, -1, 490, 388, 390, -1, 491, 398, 489, -1, 490, 390, 492, -1, 390, 493, 492, -1, 493, 490, 492, -1, 490, 493, 494, -1, 493, 495, 494, -1, 495, 490, 494, -1, 490, 495, 496, -1, 495, 384, 496, -1, 384, 490, 496, -1, 394, 396, 497, -1, 497, 396, 498, -1, 396, 398, 498, -1, 398, 497, 498, -1, 497, 398, 491, -1, 390, 392, 499, -1, 499, 392, 394, -1, 499, 394, 497, -1, 493, 390, 499, -1, 381, 382, 482, -1, 382, 384, 482, -1, 482, 384, 495, -1, 407, 408, 500, -1, 407, 500, 501, -1, 408, 409, 502, -1, 502, 500, 408, -1, 503, 217, 399, -1, 503, 214, 217, -1, 409, 410, 504, -1, 409, 504, 502, -1, 505, 399, 400, -1, 506, 410, 411, -1, 399, 505, 503, -1, 506, 504, 410, -1, 507, 400, 401, -1, 507, 505, 400, -1, 508, 401, 402, -1, 401, 508, 507, -1, 402, 403, 509, -1, 402, 509, 508, -1, 510, 403, 404, -1, 510, 509, 403, -1, 404, 405, 511, -1, 511, 510, 404, -1, 405, 406, 512, -1, 406, 501, 512, -1, 405, 512, 511, -1, 406, 407, 501, -1, 213, 211, 465, -1, 465, 210, 466, -1, 466, 513, 514, -1, 466, 210, 513, -1, 210, 207, 513, -1, 412, 515, 411, -1, 413, 515, 412, -1, 467, 466, 514, -1, 467, 514, 516, -1, 467, 516, 517, -1, 515, 518, 411, -1, 468, 467, 517, -1, 468, 517, 519, -1, 468, 519, 520, -1, 414, 515, 413, -1, 469, 468, 520, -1, 469, 520, 521, -1, 469, 521, 522, -1, 472, 471, 146, -1, 146, 471, 523, -1, 524, 146, 523, -1, 518, 146, 524, -1, 472, 146, 145, -1, 211, 201, 525, -1, 470, 469, 522, -1, 470, 522, 526, -1, 201, 205, 525, -1, 470, 526, 523, -1, 471, 470, 523, -1, 211, 210, 465, -1, 518, 515, 146, -1, 202, 527, 200, -1, 202, 528, 527, -1, 529, 528, 202, -1, 202, 530, 529, -1, 530, 202, 152, -1, 531, 530, 152, -1, 532, 531, 152, -1, 533, 532, 152, -1, 192, 317, 218, -1, 192, 194, 317, -1, 196, 435, 433, -1, 433, 170, 196, -1, 195, 194, 193, -1, 197, 191, 190, -1, 199, 198, 172, -1, 171, 199, 172, -1, 169, 534, 168, -1, 168, 534, 535, -1, 168, 535, 536, -1, 168, 536, 120, -1, 168, 120, 119, -1, 537, 168, 119, -1, 538, 168, 537, -1, 539, 168, 538, -1, 540, 168, 539, -1, 540, 541, 168, -1, 541, 542, 168, -1, 542, 543, 168, -1, 165, 168, 543, -1, 164, 113, 544, -1, 433, 164, 544, -1, 545, 434, 326, -1, 545, 546, 434, -1, 545, 326, 437, -1, 323, 196, 438, -1, 323, 435, 196, -1, 439, 547, 437, -1, 196, 193, 548, -1, 193, 549, 548, -1, 549, 196, 548, -1, 193, 550, 551, -1, 550, 549, 551, -1, 549, 193, 551, -1, 550, 547, 549, -1, 438, 196, 552, -1, 196, 549, 552, -1, 549, 438, 552, -1, 547, 439, 549, -1, 439, 438, 549, -1, 434, 553, 440, -1, 414, 553, 554, -1, 555, 554, 553, -1, 143, 555, 553, -1, 143, 553, 434, -1, 440, 553, 414, -1, 556, 225, 221, -1, 445, 556, 221, -1, 445, 225, 556, -1, 160, 460, 459, -1, 160, 202, 460, -1, 452, 453, 145, -1, 473, 472, 145, -1, 474, 473, 145, -1, 464, 201, 212, -1, 453, 474, 145, -1, 463, 201, 464, -1, 152, 202, 160, -1, 462, 201, 463, -1, 461, 201, 462, -1, 161, 454, 452, -1, 460, 202, 461, -1, 161, 455, 454, -1, 161, 456, 455, -1, 161, 162, 456, -1, 202, 201, 461, -1, 161, 452, 145, -1, 111, 161, 145, -1, 557, 145, 558, -1, 559, 145, 557, -1, 559, 111, 145, -1, 160, 457, 162, -1, 160, 458, 457, -1, 160, 459, 458, -1, 486, 560, 487, -1, 183, 560, 486, -1, 183, 185, 560, -1, 487, 560, 488, -1, 560, 185, 377, -1, 488, 560, 377, -1, 489, 561, 491, -1, 485, 561, 489, -1, 215, 159, 216, -1, 216, 159, 485, -1, 215, 156, 159, -1, 561, 485, 562, -1, 485, 159, 562, -1, 159, 561, 562, -1, 563, 159, 158, -1, 159, 563, 561, -1, 564, 486, 483, -1, 178, 564, 483, -1, 178, 182, 564, -1, 564, 183, 486, -1, 182, 183, 564, -1, 481, 178, 483, -1, 181, 178, 481, -1, 482, 565, 481, -1, 566, 565, 482, -1, 566, 96, 565, -1, 96, 567, 565, -1, 565, 567, 181, -1, 181, 481, 568, -1, 481, 565, 568, -1, 565, 181, 568, -1, 493, 569, 495, -1, 493, 570, 571, -1, 570, 569, 571, -1, 569, 493, 571, -1, 570, 566, 569, -1, 569, 482, 495, -1, 566, 482, 569, -1, 499, 572, 493, -1, 572, 570, 493, -1, 102, 572, 499, -1, 499, 497, 573, -1, 497, 574, 573, -1, 574, 499, 573, -1, 158, 574, 561, -1, 497, 561, 574, -1, 103, 574, 158, -1, 103, 102, 574, -1, 102, 499, 575, -1, 499, 574, 575, -1, 574, 102, 575, -1, 561, 497, 491, -1, 156, 215, 576, -1, 577, 156, 576, -1, 578, 508, 509, -1, 578, 509, 579, -1, 580, 508, 578, -1, 580, 507, 508, -1, 581, 507, 580, -1, 581, 505, 507, -1, 504, 582, 502, -1, 583, 505, 581, -1, 584, 500, 502, -1, 582, 584, 502, -1, 583, 503, 505, -1, 506, 585, 504, -1, 586, 503, 587, -1, 503, 583, 587, -1, 583, 586, 587, -1, 585, 582, 504, -1, 586, 214, 503, -1, 576, 214, 586, -1, 588, 501, 500, -1, 584, 588, 500, -1, 589, 585, 506, -1, 590, 512, 501, -1, 588, 590, 501, -1, 576, 215, 214, -1, 591, 511, 512, -1, 590, 591, 512, -1, 579, 510, 511, -1, 591, 579, 511, -1, 579, 509, 510, -1, 577, 576, 592, -1, 593, 577, 592, -1, 589, 411, 518, -1, 589, 506, 411, -1, 206, 208, 592, -1, 576, 206, 592, -1, 208, 209, 594, -1, 594, 209, 210, -1, 208, 594, 592, -1, 592, 594, 593, -1, 595, 594, 596, -1, 595, 597, 594, -1, 593, 594, 597, -1, 598, 211, 525, -1, 598, 525, 599, -1, 598, 600, 211, -1, 596, 211, 600, -1, 594, 210, 601, -1, 601, 210, 602, -1, 210, 211, 602, -1, 211, 601, 602, -1, 596, 594, 601, -1, 601, 211, 603, -1, 211, 596, 603, -1, 596, 601, 603, -1, 604, 599, 605, -1, 599, 525, 605, -1, 525, 604, 605, -1, 525, 205, 604, -1, 599, 604, 606, -1, 604, 205, 204, -1, 606, 604, 204, -1, 203, 200, 179, -1, 180, 203, 179, -1, 607, 606, 204, -1, 607, 204, 203, -1, 606, 607, 79, -1, 608, 79, 607, -1, 608, 607, 180, -1, 607, 203, 609, -1, 203, 180, 609, -1, 180, 607, 609, -1, 610, 179, 611, -1, 179, 200, 611, -1, 200, 610, 611, -1, 610, 200, 612, -1, 200, 527, 612, -1, 527, 610, 612, -1, 179, 610, 177, -1, 527, 528, 610, -1, 184, 177, 610, -1, 610, 528, 613, -1, 528, 184, 613, -1, 184, 610, 613, -1, 155, 614, 153, -1, 155, 174, 614, -1, 533, 614, 174, -1, 174, 532, 533, -1, 532, 174, 172, -1, 531, 532, 172, -1, 531, 172, 198, -1, 198, 530, 531, -1, 198, 190, 530, -1, 529, 530, 190, -1, 529, 190, 188, -1, 528, 529, 188, -1, 188, 184, 528, -1, 614, 533, 152, -1, 614, 152, 153, -1, 153, 152, 151, -1, 554, 142, 414, -1, 555, 142, 554, -1, 143, 142, 555, -1, 142, 146, 515, -1, 142, 515, 414, -1, 523, 584, 582, -1, 524, 523, 582, -1, 576, 207, 206, -1, 582, 585, 524, -1, 585, 518, 524, -1, 207, 576, 586, -1, 518, 585, 589, -1, 586, 513, 207, -1, 586, 583, 513, -1, 514, 513, 583, -1, 583, 581, 514, -1, 516, 514, 581, -1, 581, 580, 516, -1, 580, 517, 516, -1, 580, 578, 517, -1, 519, 517, 578, -1, 578, 579, 519, -1, 520, 519, 579, -1, 579, 591, 520, -1, 591, 521, 520, -1, 591, 590, 521, -1, 522, 521, 590, -1, 590, 588, 522, -1, 588, 526, 522, -1, 526, 588, 584, -1, 523, 526, 584, -1, 544, 615, 433, -1, 433, 616, 617, -1, 433, 618, 616, -1, 615, 618, 433, -1, 433, 617, 170, -1, 170, 619, 141, -1, 619, 170, 620, -1, 620, 170, 621, -1, 621, 170, 622, -1, 622, 170, 623, -1, 624, 623, 170, -1, 625, 624, 170, -1, 626, 625, 170, -1, 627, 626, 170, -1, 628, 627, 170, -1, 629, 628, 170, -1, 630, 629, 170, -1, 170, 617, 630, -1, 154, 151, 150, -1, 550, 193, 106, -1, 193, 154, 631, -1, 150, 631, 154, -1, 106, 631, 150, -1, 106, 193, 631, -1, 632, 180, 181, -1, 72, 632, 181, -1, 139, 169, 141, -1, 118, 540, 539, -1, 541, 540, 118, -1, 542, 541, 118, -1, 543, 542, 118, -1, 165, 543, 118, -1, 633, 165, 118, -1, 113, 167, 116, -1, 634, 117, 120, -1, 166, 163, 635, -1, 634, 120, 536, -1, 167, 166, 635, -1, 116, 167, 635, -1, 634, 536, 535, -1, 636, 634, 535, -1, 636, 535, 534, -1, 637, 636, 534, -1, 637, 534, 169, -1, 118, 117, 638, -1, 163, 165, 633, -1, 639, 118, 638, -1, 640, 118, 639, -1, 640, 641, 118, -1, 635, 163, 633, -1, 641, 633, 118, -1, 118, 537, 119, -1, 118, 538, 537, -1, 118, 539, 538, -1, 642, 544, 113, -1, 113, 643, 642, -1, 642, 643, 644, -1, 645, 642, 644, -1, 646, 645, 644, -1, 646, 644, 647, -1, 647, 648, 646, -1, 649, 646, 648, -1, 650, 649, 648, -1, 648, 651, 650, -1, 651, 652, 650, -1, 652, 653, 650, -1, 654, 653, 652, -1, 654, 652, 655, -1, 656, 654, 655, -1, 655, 657, 656, -1, 658, 656, 657, -1, 657, 659, 658, -1, 660, 658, 659, -1, 660, 659, 661, -1, 662, 660, 661, -1, 662, 661, 663, -1, 663, 664, 662, -1, 664, 663, 665, -1, 665, 666, 664, -1, 665, 667, 666, -1, 667, 668, 666, -1, 667, 669, 668, -1, 670, 668, 669, -1, 669, 671, 670, -1, 672, 670, 671, -1, 671, 673, 672, -1, 674, 672, 673, -1, 674, 673, 675, -1, 676, 674, 675, -1, 676, 675, 677, -1, 677, 678, 676, -1, 678, 679, 676, -1, 680, 679, 678, -1, 678, 681, 680, -1, 682, 680, 681, -1, 682, 681, 683, -1, 683, 684, 682, -1, 684, 683, 685, -1, 686, 684, 685, -1, 685, 687, 686, -1, 687, 57, 686, -1, 57, 687, 688, -1, 57, 688, 689, -1, 56, 57, 689, -1, 56, 689, 121, -1, 54, 56, 121, -1, 546, 690, 434, -1, 109, 558, 690, -1, 109, 690, 546, -1, 434, 690, 143, -1, 691, 143, 690, -1, 692, 691, 690, -1, 693, 692, 690, -1, 694, 693, 690, -1, 695, 694, 690, -1, 558, 695, 690, -1, 437, 546, 545, -1, 110, 546, 437, -1, 550, 106, 110, -1, 550, 110, 437, -1, 547, 550, 437, -1, 152, 696, 107, -1, 105, 696, 111, -1, 104, 696, 105, -1, 107, 696, 104, -1, 111, 696, 160, -1, 152, 160, 696, -1, 111, 697, 109, -1, 109, 697, 558, -1, 557, 558, 697, -1, 559, 557, 697, -1, 111, 559, 697, -1, 691, 145, 143, -1, 692, 145, 691, -1, 693, 145, 692, -1, 694, 145, 693, -1, 695, 145, 694, -1, 558, 145, 695, -1, 157, 156, 99, -1, 698, 157, 99, -1, 698, 99, 103, -1, 101, 99, 156, -1, 699, 156, 700, -1, 699, 101, 156, -1, 158, 698, 103, -1, 157, 698, 158, -1, 563, 158, 561, -1, 98, 701, 102, -1, 702, 703, 102, -1, 701, 702, 102, -1, 102, 703, 572, -1, 570, 92, 566, -1, 566, 92, 96, -1, 40, 92, 570, -1, 91, 704, 96, -1, 704, 88, 96, -1, 705, 90, 88, -1, 88, 704, 705, -1, 703, 40, 570, -1, 572, 703, 570, -1, 87, 181, 88, -1, 706, 181, 707, -1, 707, 181, 708, -1, 708, 181, 87, -1, 567, 88, 181, -1, 88, 567, 96, -1, 706, 72, 181, -1, 577, 709, 156, -1, 156, 709, 700, -1, 577, 593, 709, -1, 700, 709, 710, -1, 709, 593, 710, -1, 593, 711, 710, -1, 712, 711, 593, -1, 593, 597, 713, -1, 597, 595, 713, -1, 595, 596, 713, -1, 596, 600, 713, -1, 713, 714, 715, -1, 713, 715, 716, -1, 713, 716, 712, -1, 713, 600, 714, -1, 712, 593, 713, -1, 717, 714, 600, -1, 718, 600, 719, -1, 717, 600, 718, -1, 719, 600, 598, -1, 599, 606, 80, -1, 606, 79, 80, -1, 599, 80, 86, -1, 81, 79, 78, -1, 719, 599, 86, -1, 598, 599, 719, -1, 180, 74, 73, -1, 180, 33, 32, -1, 180, 32, 720, -1, 180, 720, 74, -1, 608, 180, 73, -1, 73, 79, 608, -1, 632, 33, 180, -1, 147, 107, 721, -1, 149, 147, 721, -1, 150, 149, 721, -1, 150, 721, 106, -1, 106, 721, 107, -1, 722, 623, 624, -1, 722, 624, 625, -1, 626, 722, 625, -1, 627, 722, 626, -1, 628, 722, 627, -1, 629, 722, 628, -1, 630, 722, 629, -1, 617, 722, 630, -1, 723, 722, 617, -1, 724, 725, 722, -1, 726, 724, 722, -1, 727, 726, 722, -1, 723, 727, 722, -1, 722, 728, 623, -1, 544, 729, 615, -1, 730, 729, 544, -1, 731, 732, 544, -1, 732, 730, 544, -1, 618, 733, 616, -1, 615, 733, 618, -1, 728, 622, 623, -1, 728, 734, 622, -1, 729, 733, 615, -1, 734, 621, 622, -1, 734, 620, 621, -1, 734, 735, 620, -1, 616, 723, 617, -1, 735, 619, 620, -1, 735, 736, 619, -1, 733, 723, 616, -1, 736, 141, 619, -1, 728, 722, 725, -1, 72, 70, 632, -1, 643, 113, 112, -1, 644, 643, 112, -1, 647, 644, 112, -1, 648, 647, 112, -1, 651, 648, 112, -1, 652, 651, 737, -1, 655, 652, 737, -1, 651, 112, 737, -1, 657, 655, 738, -1, 655, 737, 738, -1, 659, 657, 739, -1, 661, 659, 739, -1, 657, 738, 739, -1, 663, 661, 740, -1, 661, 739, 740, -1, 665, 663, 741, -1, 667, 665, 741, -1, 663, 740, 741, -1, 667, 741, 742, -1, 669, 667, 742, -1, 669, 742, 743, -1, 671, 669, 743, -1, 673, 671, 743, -1, 673, 743, 744, -1, 675, 673, 744, -1, 675, 744, 677, -1, 677, 744, 745, -1, 677, 745, 678, -1, 678, 745, 746, -1, 678, 746, 681, -1, 681, 746, 683, -1, 746, 747, 683, -1, 683, 747, 685, -1, 747, 748, 685, -1, 685, 748, 687, -1, 687, 748, 688, -1, 748, 749, 688, -1, 688, 749, 689, -1, 749, 750, 689, -1, 689, 750, 121, -1, 637, 169, 139, -1, 751, 637, 139, -1, 121, 750, 122, -1, 750, 752, 122, -1, 751, 139, 137, -1, 753, 751, 137, -1, 122, 752, 123, -1, 752, 754, 123, -1, 753, 137, 135, -1, 123, 754, 124, -1, 753, 135, 133, -1, 755, 753, 133, -1, 124, 754, 125, -1, 754, 756, 125, -1, 755, 133, 131, -1, 757, 755, 131, -1, 125, 756, 126, -1, 756, 758, 126, -1, 757, 131, 129, -1, 126, 758, 127, -1, 127, 758, 128, -1, 758, 757, 128, -1, 757, 129, 128, -1, 642, 731, 544, -1, 645, 731, 642, -1, 646, 731, 645, -1, 649, 731, 646, -1, 650, 731, 649, -1, 653, 759, 650, -1, 653, 654, 759, -1, 650, 759, 731, -1, 654, 656, 760, -1, 654, 760, 759, -1, 656, 658, 761, -1, 658, 660, 761, -1, 656, 761, 760, -1, 660, 662, 762, -1, 660, 762, 761, -1, 662, 664, 763, -1, 664, 666, 763, -1, 662, 763, 762, -1, 666, 764, 763, -1, 666, 668, 764, -1, 668, 765, 764, -1, 668, 670, 765, -1, 670, 672, 765, -1, 672, 766, 765, -1, 672, 674, 766, -1, 674, 676, 766, -1, 676, 767, 766, -1, 676, 679, 767, -1, 767, 679, 768, -1, 679, 680, 768, -1, 680, 682, 768, -1, 768, 682, 769, -1, 682, 684, 769, -1, 769, 684, 770, -1, 684, 686, 770, -1, 686, 57, 770, -1, 770, 57, 55, -1, 56, 54, 53, -1, 736, 140, 141, -1, 736, 771, 140, -1, 771, 138, 140, -1, 771, 772, 138, -1, 772, 136, 138, -1, 51, 49, 48, -1, 772, 134, 136, -1, 772, 773, 134, -1, 773, 132, 134, -1, 773, 43, 132, -1, 43, 130, 132, -1, 46, 44, 41, -1, 43, 42, 130, -1, 752, 750, 53, -1, 752, 53, 50, -1, 754, 752, 48, -1, 752, 50, 48, -1, 756, 754, 45, -1, 754, 48, 45, -1, 758, 756, 41, -1, 45, 41, 756, -1, 43, 757, 758, -1, 41, 43, 758, -1, 773, 755, 757, -1, 43, 773, 757, -1, 773, 753, 755, -1, 773, 772, 753, -1, 772, 771, 753, -1, 771, 751, 753, -1, 771, 736, 751, -1, 736, 637, 751, -1, 736, 636, 637, -1, 636, 736, 735, -1, 735, 734, 636, -1, 734, 634, 636, -1, 117, 634, 734, -1, 734, 728, 117, -1, 728, 638, 117, -1, 638, 728, 725, -1, 639, 638, 725, -1, 639, 725, 724, -1, 640, 639, 724, -1, 640, 724, 726, -1, 726, 641, 640, -1, 641, 726, 727, -1, 727, 633, 641, -1, 633, 727, 723, -1, 723, 635, 633, -1, 635, 723, 733, -1, 733, 116, 635, -1, 116, 733, 729, -1, 115, 116, 729, -1, 729, 730, 115, -1, 730, 732, 115, -1, 114, 115, 732, -1, 732, 112, 114, -1, 732, 731, 112, -1, 737, 112, 731, -1, 737, 731, 759, -1, 738, 737, 760, -1, 737, 759, 760, -1, 739, 738, 761, -1, 760, 761, 738, -1, 762, 740, 739, -1, 739, 761, 762, -1, 741, 740, 763, -1, 740, 762, 763, -1, 764, 742, 741, -1, 741, 763, 764, -1, 765, 743, 742, -1, 764, 765, 742, -1, 766, 744, 743, -1, 743, 765, 766, -1, 767, 745, 744, -1, 766, 767, 744, -1, 745, 767, 768, -1, 768, 746, 745, -1, 746, 768, 769, -1, 747, 746, 769, -1, 769, 770, 747, -1, 748, 747, 770, -1, 748, 770, 55, -1, 749, 748, 55, -1, 55, 53, 749, -1, 750, 749, 53, -1, 110, 109, 774, -1, 110, 774, 546, -1, 546, 774, 109, -1, 97, 775, 98, -1, 699, 775, 101, -1, 700, 775, 699, -1, 100, 776, 97, -1, 101, 776, 100, -1, 101, 775, 776, -1, 776, 775, 97, -1, 94, 777, 93, -1, 95, 777, 94, -1, 38, 777, 95, -1, 778, 91, 93, -1, 777, 778, 93, -1, 91, 779, 704, -1, 778, 779, 91, -1, 704, 779, 705, -1, 98, 780, 701, -1, 705, 779, 90, -1, 98, 781, 780, -1, 702, 782, 703, -1, 701, 782, 702, -1, 701, 780, 782, -1, 775, 783, 98, -1, 783, 781, 98, -1, 703, 784, 40, -1, 782, 784, 703, -1, 775, 785, 783, -1, 784, 37, 40, -1, 39, 38, 95, -1, 779, 786, 90, -1, 786, 787, 90, -1, 90, 787, 89, -1, 788, 706, 707, -1, 789, 788, 707, -1, 788, 790, 706, -1, 790, 72, 706, -1, 791, 707, 708, -1, 791, 708, 87, -1, 791, 87, 89, -1, 787, 791, 89, -1, 792, 789, 791, -1, 791, 793, 794, -1, 793, 792, 794, -1, 792, 791, 794, -1, 787, 793, 791, -1, 791, 789, 707, -1, 710, 795, 700, -1, 710, 796, 795, -1, 796, 13, 795, -1, 13, 796, 797, -1, 798, 13, 797, -1, 797, 799, 798, -1, 799, 800, 798, -1, 800, 799, 801, -1, 802, 800, 801, -1, 802, 801, 803, -1, 802, 803, 804, -1, 805, 802, 804, -1, 806, 805, 804, -1, 806, 804, 807, -1, 807, 808, 806, -1, 808, 809, 806, -1, 18, 809, 808, -1, 18, 808, 29, -1, 29, 28, 18, -1, 3, 18, 28, -1, 810, 715, 714, -1, 711, 712, 810, -1, 710, 711, 810, -1, 811, 716, 715, -1, 712, 716, 811, -1, 712, 811, 810, -1, 811, 715, 810, -1, 83, 82, 812, -1, 84, 83, 812, -1, 813, 84, 812, -1, 814, 82, 81, -1, 812, 82, 814, -1, 81, 78, 815, -1, 814, 81, 815, -1, 815, 78, 77, -1, 816, 714, 717, -1, 815, 77, 76, -1, 817, 714, 816, -1, 718, 719, 818, -1, 717, 718, 818, -1, 816, 717, 818, -1, 819, 810, 714, -1, 817, 819, 714, -1, 719, 86, 820, -1, 818, 719, 820, -1, 821, 810, 819, -1, 86, 85, 822, -1, 822, 85, 813, -1, 820, 86, 822, -1, 85, 84, 813, -1, 823, 815, 76, -1, 823, 76, 824, -1, 824, 76, 75, -1, 825, 30, 33, -1, 825, 33, 632, -1, 826, 720, 32, -1, 826, 74, 720, -1, 826, 75, 74, -1, 824, 75, 826, -1, 827, 826, 31, -1, 827, 828, 829, -1, 828, 826, 829, -1, 826, 827, 829, -1, 828, 824, 826, -1, 31, 826, 32, -1, 796, 710, 830, -1, 831, 70, 69, -1, 832, 831, 69, -1, 710, 810, 830, -1, 796, 830, 797, -1, 832, 69, 67, -1, 833, 832, 67, -1, 797, 830, 834, -1, 834, 830, 835, -1, 799, 797, 836, -1, 836, 797, 834, -1, 24, 25, 58, -1, 58, 25, 837, -1, 833, 67, 65, -1, 801, 799, 838, -1, 58, 837, 59, -1, 838, 799, 836, -1, 803, 801, 838, -1, 833, 65, 63, -1, 839, 833, 63, -1, 840, 803, 838, -1, 59, 837, 60, -1, 837, 841, 60, -1, 28, 27, 34, -1, 27, 21, 34, -1, 804, 803, 840, -1, 839, 63, 61, -1, 841, 839, 61, -1, 60, 841, 61, -1, 804, 840, 842, -1, 804, 842, 807, -1, 807, 842, 843, -1, 807, 843, 808, -1, 34, 21, 35, -1, 843, 26, 29, -1, 831, 825, 70, -1, 825, 632, 70, -1, 808, 843, 29, -1, 35, 21, 23, -1, 844, 68, 71, -1, 795, 10, 700, -1, 845, 66, 68, -1, 10, 775, 700, -1, 845, 846, 66, -1, 13, 10, 795, -1, 846, 64, 66, -1, 14, 9, 8, -1, 798, 847, 13, -1, 847, 11, 13, -1, 846, 62, 64, -1, 6, 62, 846, -1, 800, 848, 798, -1, 848, 847, 798, -1, 6, 5, 62, -1, 802, 848, 800, -1, 802, 849, 848, -1, 1, 36, 2, -1, 805, 849, 802, -1, 805, 850, 849, -1, 805, 806, 850, -1, 850, 806, 851, -1, 806, 809, 851, -1, 790, 844, 71, -1, 790, 71, 72, -1, 2, 20, 19, -1, 851, 18, 17, -1, 36, 20, 2, -1, 809, 18, 851, -1, 844, 845, 68, -1, 22, 21, 2, -1, 2, 19, 22, -1, 25, 22, 15, -1, 22, 19, 15, -1, 837, 25, 8, -1, 15, 8, 25, -1, 4, 841, 837, -1, 8, 4, 837, -1, 839, 841, 6, -1, 841, 4, 6, -1, 839, 6, 846, -1, 846, 833, 839, -1, 832, 833, 846, -1, 846, 845, 832, -1, 832, 845, 844, -1, 844, 831, 832, -1, 831, 844, 790, -1, 790, 825, 831, -1, 790, 788, 825, -1, 788, 30, 825, -1, 30, 788, 789, -1, 789, 31, 30, -1, 792, 827, 31, -1, 31, 789, 792, -1, 828, 827, 793, -1, 827, 792, 793, -1, 824, 828, 787, -1, 793, 787, 828, -1, 823, 824, 786, -1, 824, 787, 786, -1, 815, 823, 779, -1, 823, 786, 779, -1, 814, 815, 778, -1, 779, 778, 815, -1, 812, 814, 777, -1, 778, 777, 814, -1, 813, 812, 38, -1, 777, 38, 812, -1, 822, 813, 37, -1, 38, 37, 813, -1, 784, 820, 822, -1, 822, 37, 784, -1, 818, 820, 782, -1, 820, 784, 782, -1, 780, 816, 818, -1, 782, 780, 818, -1, 781, 817, 816, -1, 780, 781, 816, -1, 783, 819, 817, -1, 781, 783, 817, -1, 785, 821, 819, -1, 819, 783, 785, -1, 785, 852, 821, -1, 852, 785, 853, -1, 853, 854, 852, -1, 854, 855, 852, -1, 854, 12, 855, -1, 835, 855, 12, -1, 834, 835, 11, -1, 12, 11, 835, -1, 11, 836, 834, -1, 836, 11, 847, -1, 838, 836, 848, -1, 836, 847, 848, -1, 840, 838, 849, -1, 838, 848, 849, -1, 850, 842, 840, -1, 849, 850, 840, -1, 851, 843, 842, -1, 842, 850, 851, -1, 17, 26, 843, -1, 851, 17, 843, -1, 0, 27, 26, -1, 26, 17, 0, -1, 21, 27, 0, -1, 21, 0, 2, -1, 10, 12, 856, -1, 775, 10, 856, -1, 775, 856, 785, -1, 856, 853, 785, -1, 856, 854, 853, -1, 856, 12, 854, -1, 835, 830, 857, -1, 830, 810, 857, -1, 857, 810, 821, -1, 857, 821, 852, -1, 857, 852, 855, -1, 857, 855, 835, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 10.500 -0.001 63.772 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 10.500 -0.001 -104.772 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 10.500 -84.273 -20.500 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 10.500 84.270 -20.500 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -73.772 -0.001 -20.500 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 94.772 -0.001 -20.500 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_KNEE_P.wrl000066400000000000000000001572021207742442300230460ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 27.586 41.250 81.338 center 0.000 0.000 0.000 #translation -5.793 -4.625 25.031 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.50 0.50 0.50 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 7.871 -12.900 1.428, 8.000 -13.100 -0.000, 8.000 -12.900 -0.000, 7.789 -13.100 1.826, 7.490 -12.900 2.811, 7.167 -13.100 3.555, 6.868 -12.900 4.103, 6.166 -13.100 5.097, 6.025 -12.900 5.264, 4.840 -13.100 6.370, 4.988 -12.900 6.255, 3.791 -12.900 7.045, 3.258 -13.100 7.306, 2.472 -12.900 7.608, 1.074 -12.900 7.928, 1.505 -13.100 7.857, -0.359 -12.900 7.992, -0.328 -13.100 7.993, -0.328 -15.700 7.993, -1.780 -12.900 7.799, -2.129 -15.700 7.712, -3.144 -12.900 7.356, -3.819 -15.700 7.030, -4.407 -12.900 6.677, -5.310 -15.700 5.983, -5.529 -12.900 5.782, -6.527 -15.700 4.626, -6.472 -12.900 4.702, -7.208 -12.900 3.471, -7.404 -15.700 3.030, -7.712 -12.900 2.128, -7.898 -15.700 1.275, -7.968 -12.900 0.717, -7.981 -15.700 -0.545, -7.968 -12.900 -0.717, -7.712 -12.900 -2.128, -7.651 -15.700 -2.337, -7.208 -12.900 -3.471, -6.924 -15.700 -4.008, -6.924 -13.100 -4.008, -6.472 -12.900 -4.702, -5.529 -12.900 -5.782, -5.652 -13.100 -5.662, -4.407 -12.900 -6.677, -3.144 -12.900 -7.356, -3.996 -13.100 -6.930, -1.780 -12.900 -7.799, -2.068 -13.100 -7.728, -0.359 -12.900 -7.992, -0.000 -13.100 -8.000, 1.074 -12.900 -7.928, 1.780 -13.100 -7.799, 2.472 -12.900 -7.608, 3.471 -13.100 -7.208, 3.791 -12.900 -7.045, 4.988 -13.100 -6.255, 4.988 -12.900 -6.255, 6.025 -12.900 -5.264, 6.255 -13.100 -4.988, 6.868 -12.900 -4.103, 7.208 -13.100 -3.471, 7.490 -12.900 -2.811, 7.799 -13.100 -1.780, 7.871 -12.900 -1.428, 19.151 -13.100 -5.946, 18.604 -13.100 -5.978, 18.601 -13.100 -5.978, 18.600 -13.100 -6.026, 10.157 -13.100 -5.257, 6.255 -13.100 -4.988, 7.208 -13.100 -3.471, 10.104 -13.100 -6.756, -6.924 -13.100 -4.008, -3.991 -13.100 -13.375, -4.095 -13.100 -14.565, -5.652 -13.100 -5.662, 7.799 -13.100 -1.780, -3.996 -13.100 -6.930, 17.187 -13.100 -1.001, 17.109 -13.100 -5.499, 8.000 -13.100 -0.000, 7.789 -13.100 1.826, 7.167 -13.100 3.555, -2.068 -13.100 -7.728, -0.000 -13.100 -8.000, 18.687 -13.100 -1.027, 19.210 -13.100 -5.958, 19.586 -13.100 15.638, 1.780 -13.100 -7.799, 6.166 -13.100 5.097, 4.840 -13.100 6.370, 3.258 -13.100 7.306, 1.505 -13.100 7.857, 16.855 -13.100 -6.991, 15.906 -13.100 -11.457, 14.154 -13.100 -11.396, -0.328 -13.100 7.993, 16.648 -13.100 -15.181, 18.594 -13.100 -6.026, 17.178 -13.100 -15.227, 15.142 -13.100 -15.049, 14.102 -13.100 -12.895, 3.471 -13.100 -7.208, 15.589 -13.100 -12.947, 4.988 -13.100 -6.255, 18.600 -13.100 -5.994, -2.828 -12.900 -2.828, -2.000 -12.900 -3.464, -3.464 -12.900 -2.000, -1.035 -12.900 -3.864, -3.864 -12.900 -1.035, -4.000 -12.900 0.000, -0.000 -12.900 -4.000, -3.864 -12.900 1.035, 1.035 -12.900 -3.864, -3.464 -12.900 2.000, 2.000 -12.900 -3.464, -2.828 -12.900 2.828, 2.828 -12.900 -2.828, 3.464 -12.900 -2.000, -2.000 -12.900 3.464, -1.035 -12.900 3.864, 3.864 -12.900 -1.035, 4.000 -12.900 0.000, -0.000 -12.900 4.000, 1.035 -12.900 3.864, 3.864 -12.900 1.035, 2.000 -12.900 3.464, 3.464 -12.900 2.000, 2.828 -12.900 2.828, -4.686 -15.700 6.321, -4.686 -15.900 6.321, 19.586 -15.900 15.638, 7.450 -14.500 10.979, 19.586 -13.100 15.638, -7.188 -15.700 -3.020, -7.188 -15.900 -3.020, -4.095 -15.900 -14.565, -4.095 -13.100 -14.565, -5.642 -14.500 -8.792, 10.104 15.100 -6.756, 16.855 15.100 -6.991, 10.157 15.100 -5.257, 17.109 15.100 -5.499, 17.187 15.100 -1.001, 18.687 15.100 -1.027, 18.601 15.100 -5.978, 18.604 15.100 -5.978, 18.600 15.100 -5.994, 18.600 15.100 -6.026, 18.594 15.100 -6.026, 17.330 1.000 -11.969, 16.067 -13.100 -17.912, 16.067 15.100 -17.912, 16.648 15.100 -15.181, 16.550 -13.100 -18.181, 14.600 -13.100 -17.600, -3.219 -13.100 -17.836, 6.526 -13.100 -15.782, 7.659 -15.700 -60.010, 7.542 -13.100 -60.560, 7.659 -13.100 -60.010, 7.542 -15.900 -60.560, 7.937 -15.700 -58.705, 17.178 -13.100 -15.227, 16.550 -15.900 -18.181, 16.550 -13.100 -18.181, 17.178 -15.900 -15.227, 19.151 -15.900 -5.946, 19.151 -13.100 -5.946, 13.347 -14.500 -33.253, 7.937 -13.100 -58.705, 19.210 -15.900 -5.958, 19.210 -13.100 -5.958, 19.210 -15.900 -5.958, 19.210 -13.100 -5.958, 15.589 15.100 -12.947, 15.094 1.000 -15.274, 15.142 15.100 -15.049, 14.600 15.100 -17.600, 14.102 15.100 -12.895, 14.154 15.100 -11.396, 15.906 15.100 -11.457, -2.828 -15.900 2.828, -3.464 -15.900 2.000, 3.464 -15.900 -2.000, 2.828 -15.900 -2.828, 4.000 -15.900 0.000, -3.864 -15.900 1.035, 3.864 -15.900 -1.035, 3.864 -15.900 1.035, -4.000 -15.900 0.000, 3.464 -15.900 2.000, -3.864 -15.900 -1.035, 2.828 -15.900 2.828, -3.464 -15.900 -2.000, 2.000 -15.900 3.464, -2.828 -15.900 -2.828, 1.035 -15.900 3.864, -2.000 -15.900 -3.464, -0.000 -15.900 4.000, -1.035 -15.900 -3.864, -1.035 -15.900 3.864, -0.000 -15.900 -4.000, -2.000 -15.900 3.464, 1.035 -15.900 -3.864, 2.000 -15.900 -3.464, 2.000 -15.900 3.464, 2.828 -15.900 2.828, 19.586 -15.900 15.638, 1.035 -15.900 3.864, 2.828 -15.900 -2.828, 10.104 -15.900 -6.756, 10.157 -15.900 -5.257, -2.828 -15.900 2.828, -2.000 -15.900 3.464, -4.686 -15.900 6.321, -3.464 -15.900 2.000, 18.600 -15.900 -6.026, 18.594 -15.900 -6.026, 16.648 -15.900 -15.181, -3.864 -15.900 1.035, 17.178 -15.900 -15.227, 19.151 -15.900 -5.946, -7.188 -15.900 -3.020, -4.095 -15.900 -14.565, -3.991 -15.900 -13.375, 18.687 -15.900 -1.027, 19.210 -15.900 -5.958, 14.102 -15.900 -12.895, 15.142 -15.900 -15.049, 17.187 -15.900 -1.001, 6.096 -15.900 0.420, 3.864 -15.900 -1.035, 3.464 -15.900 -2.000, 4.000 -15.900 0.000, 3.864 -15.900 1.035, 3.464 -15.900 2.000, -2.828 -15.900 -2.828, -3.464 -15.900 -2.000, -2.000 -15.900 -3.464, 14.154 -15.900 -11.396, 15.906 -15.900 -11.457, 16.855 -15.900 -6.991, -3.864 -15.900 -1.035, -1.035 -15.900 3.864, -1.035 -15.900 -3.864, -4.000 -15.900 0.000, 15.589 -15.900 -12.947, -0.000 -15.900 -4.000, 18.600 -15.900 -5.994, 18.604 -15.900 -5.978, 2.000 -15.900 -3.464, 1.035 -15.900 -3.864, 18.601 -15.900 -5.978, -0.000 -15.900 4.000, 17.109 -15.900 -5.499, -7.948 -13.100 -58.608, -8.000 -13.100 -59.200, -7.948 -15.700 -58.608, -8.000 -15.900 -59.200, -7.670 -15.700 -55.425, -4.095 -15.900 -14.565, -4.095 -13.100 -14.565, -6.047 -14.500 -36.883, -7.670 -13.100 -55.425, 7.208 15.100 -3.471, 6.255 15.100 -4.988, 19.151 15.100 -5.946, -4.095 15.100 -14.565, -3.991 15.100 -13.375, -6.924 15.100 -4.008, 7.799 15.100 -1.780, -5.652 15.100 -5.662, -3.996 15.100 -6.930, -2.068 15.100 -7.728, 7.789 15.100 1.826, 8.000 15.100 -0.000, 7.167 15.100 3.555, -0.000 15.100 -8.000, 1.780 15.100 -7.799, 19.210 15.100 -5.958, 19.586 15.100 15.638, 6.166 15.100 5.097, 4.840 15.100 6.370, 3.258 15.100 7.306, 1.505 15.100 7.857, -0.328 15.100 7.993, 17.178 15.100 -15.227, 3.471 15.100 -7.208, 4.988 15.100 -6.255, 16.550 15.100 -18.181, -3.219 15.100 -17.836, 6.526 15.100 -15.782, 4.436 -13.100 -36.855, 4.646 -13.100 -51.188, 2.982 -13.100 -50.276, 1.149 -13.100 -49.783, -0.748 -13.100 -49.735, -2.603 -13.100 -50.135, -4.311 -13.100 -50.961, 7.112 -13.100 -54.037, -5.777 -13.100 -52.166, -6.918 -13.100 -53.683, -7.670 -13.100 -55.425, 7.937 -13.100 -58.705, 7.972 -13.100 -58.371, 7.993 -13.100 -58.036, 8.000 -13.100 -57.700, 7.775 -13.100 -55.816, 6.050 -13.100 -52.465, 7.540 -13.100 -60.374, 7.474 -13.100 -60.554, 7.602 -13.100 -60.193, 3.142 -15.900 -60.175, 7.474 -15.700 -60.554, 3.142 -15.700 -60.175, -8.000 -15.900 -59.200, -3.524 -15.900 -59.592, -3.892 -15.900 -58.624, -2.771 -15.900 -54.816, -3.219 -15.900 -17.836, -3.999 -15.900 -57.594, -3.837 -15.900 -56.571, -3.419 -15.900 -55.624, 2.847 -15.900 -54.890, 3.473 -15.900 -55.715, 4.354 -15.900 -37.792, 2.031 -15.900 -54.254, 1.078 -15.900 -53.848, 0.053 -15.900 -53.700, -0.975 -15.900 -53.821, -1.938 -15.900 -54.201, 7.542 -15.900 -60.560, 16.550 -15.900 -18.181, 3.611 -15.900 -59.421, 3.142 -15.900 -60.175, 3.902 -15.900 -58.582, 4.000 -15.900 -57.700, 3.866 -15.900 -56.673, 16.067 -15.900 -17.912, 14.600 -15.900 -17.600, 6.526 -15.900 -15.782, 7.871 -12.900 -56.272, 8.000 -13.100 -57.700, 8.000 -12.900 -57.700, 7.775 -13.100 -55.816, 7.490 -12.900 -54.889, 7.112 -13.100 -54.037, 6.868 -12.900 -53.597, 6.050 -13.100 -52.465, 6.025 -12.900 -52.436, 4.988 -12.900 -51.445, 4.646 -13.100 -51.188, 3.791 -12.900 -50.655, 2.472 -12.900 -50.092, 2.982 -13.100 -50.276, 1.149 -13.100 -49.783, 1.074 -12.900 -49.772, -0.359 -12.900 -49.708, -0.748 -13.100 -49.735, -1.780 -12.900 -49.901, -3.144 -12.900 -50.344, -2.603 -13.100 -50.135, -4.311 -13.100 -50.961, -4.407 -12.900 -51.023, -5.529 -12.900 -51.918, -5.777 -13.100 -52.166, -6.472 -12.900 -52.998, -7.208 -12.900 -54.229, -6.918 -13.100 -53.683, -7.712 -12.900 -55.572, -7.905 -15.700 -56.471, -7.968 -12.900 -56.983, -7.998 -15.700 -57.538, -7.968 -12.900 -58.417, -7.923 -13.100 -58.811, -7.892 -13.100 -59.012, -7.856 -13.100 -59.213, -7.712 -12.900 -59.828, -7.856 -15.700 -59.213, -7.287 -15.700 -61.001, -7.208 -12.900 -61.171, -6.472 -12.900 -62.402, -6.317 -15.700 -62.608, -5.529 -12.900 -63.482, -5.000 -15.700 -63.945, -4.407 -12.900 -64.377, -3.407 -15.700 -64.938, -3.144 -12.900 -65.056, -1.627 -15.700 -65.533, -1.780 -12.900 -65.499, -0.359 -12.900 -65.692, 0.242 -15.700 -65.696, 1.074 -12.900 -65.628, 2.099 -15.700 -65.420, 2.472 -12.900 -65.308, 3.840 -15.700 -64.718, 3.791 -12.900 -64.745, 4.988 -12.900 -63.955, 5.369 -15.700 -63.631, 6.025 -12.900 -62.964, 6.603 -15.700 -62.216, 6.868 -12.900 -61.803, 7.490 -12.900 -60.511, 7.776 -15.700 -59.580, 7.871 -12.900 -59.128, 7.868 -15.700 -59.145, 7.972 -13.100 -58.371, 7.993 -13.100 -58.036, 10.104 -16.000 -6.756, 16.855 -16.000 -6.991, 10.157 -16.000 -5.257, 17.109 -16.000 -5.499, 17.187 -16.000 -1.001, 18.687 -16.000 -1.027, 18.601 -16.000 -5.978, 18.604 -16.000 -5.978, 18.600 -16.000 -5.994, 18.600 -16.000 -6.026, 18.594 -16.000 -6.026, 16.067 -16.000 -17.912, 17.330 -15.950 -11.969, 15.589 -16.000 -12.947, 14.600 -16.000 -17.600, 15.094 -15.950 -15.274, 14.102 -16.000 -12.895, 14.154 -16.000 -11.396, 15.906 -16.000 -11.457, -7.856 -15.700 -59.213, -7.856 -13.100 -59.213, -3.524 -15.900 -59.592, -3.524 -15.700 -59.592, 7.659 15.100 -60.010, 7.542 15.100 -60.560, 7.659 17.700 -60.010, 7.542 17.900 -60.560, 7.937 17.700 -58.705, 16.550 17.900 -18.181, 17.178 15.100 -15.227, 17.178 17.900 -15.227, 19.151 15.100 -5.946, 19.151 17.900 -5.946, 7.937 15.100 -58.705, 13.347 16.500 -33.253, 19.210 15.100 -5.958, 19.210 17.900 -5.958, 19.210 17.900 -5.958, 19.586 17.900 15.638, -4.686 17.900 6.321, -4.686 17.700 6.321, -0.328 17.700 7.993, 7.450 16.500 10.979, 8.000 14.900 -0.000, 7.799 14.900 1.780, 7.208 14.900 3.471, 6.255 14.900 4.988, 4.988 14.900 6.255, 3.471 14.900 7.208, 1.780 14.900 7.799, 0.000 14.900 8.000, -1.780 14.900 7.799, -2.129 17.700 7.712, -3.471 14.900 7.208, -3.819 17.700 7.030, -4.988 14.900 6.255, -5.310 17.700 5.983, -6.255 14.900 4.988, -6.527 17.700 4.626, -7.208 14.900 3.471, -7.404 17.700 3.030, -7.799 14.900 1.780, -7.898 17.700 1.275, -8.000 14.900 0.000, -7.981 17.700 -0.545, -7.799 14.900 -1.780, -7.651 17.700 -2.337, -7.208 14.900 -3.471, -6.924 17.700 -4.008, -6.255 14.900 -4.988, -4.988 14.900 -6.255, -3.471 14.900 -7.208, -1.780 14.900 -7.799, -0.000 14.900 -8.000, 1.780 14.900 -7.799, 3.471 14.900 -7.208, 4.988 14.900 -6.255, 6.255 14.900 -4.988, 7.208 14.900 -3.471, 7.799 14.900 -1.780, -7.188 17.900 -3.020, -7.188 17.700 -3.020, -4.095 17.900 -14.565, -5.642 16.500 -8.792, 2.982 15.100 -50.276, 4.646 15.100 -51.188, 4.436 15.100 -36.855, 1.149 15.100 -49.783, -0.748 15.100 -49.735, -2.603 15.100 -50.135, -4.311 15.100 -50.961, 7.112 15.100 -54.037, -5.777 15.100 -52.166, -6.918 15.100 -53.683, -7.670 15.100 -55.425, 7.972 15.100 -58.371, 7.993 15.100 -58.036, 8.000 15.100 -57.700, 7.775 15.100 -55.816, 6.050 15.100 -52.465, 2.326 -15.700 -60.954, 5.369 -15.700 -63.631, -0.191 -15.700 -62.454, 0.217 -15.700 -61.694, -0.907 -15.700 -61.596, 1.324 -15.700 -61.474, 2.099 -15.700 -65.420, -2.856 -15.700 -60.501, -1.960 -15.700 -61.187, -7.287 -15.700 -61.001, 3.864 -12.900 -56.665, 4.000 -12.900 -57.700, 3.866 -15.900 -56.673, 4.000 -15.900 -57.700, 3.464 -12.900 -55.700, 3.473 -15.900 -55.715, 2.828 -12.900 -54.872, 2.847 -15.900 -54.890, 2.000 -12.900 -54.236, 2.031 -15.900 -54.254, 1.035 -12.900 -53.836, 1.078 -15.900 -53.848, -0.000 -12.900 -53.700, 0.053 -15.900 -53.700, -1.035 -12.900 -53.836, -0.975 -15.900 -53.821, -2.000 -12.900 -54.236, -1.938 -15.900 -54.201, -2.828 -12.900 -54.872, -2.771 -15.900 -54.816, -3.464 -12.900 -55.700, -3.419 -15.900 -55.624, -3.864 -12.900 -56.665, -3.837 -15.900 -56.571, -3.999 -15.900 -57.594, -4.000 -12.900 -57.700, -3.892 -15.900 -58.624, -3.864 -12.900 -58.735, -3.464 -12.900 -59.700, -2.828 -12.900 -60.528, -2.000 -12.900 -61.164, -1.035 -12.900 -61.564, -0.000 -12.900 -61.700, 1.035 -12.900 -61.564, 2.000 -12.900 -61.164, 2.828 -12.900 -60.528, 3.464 -12.900 -59.700, 3.611 -15.900 -59.421, 3.864 -12.900 -58.735, 3.902 -15.900 -58.582, -5.529 -12.900 -63.482, -6.472 -12.900 -62.402, -7.968 -12.900 -56.983, -4.407 -12.900 -51.023, -3.144 -12.900 -50.344, 1.074 -12.900 -49.772, 7.871 -12.900 -56.272, 7.490 -12.900 -54.889, 17.103 -16.000 -5.824, 13.609 -16.000 -6.128, 17.152 -16.000 -5.501, 17.146 -16.000 -5.669, 17.894 -16.000 -3.489, 17.100 -16.000 -13.000, 17.152 -16.000 -11.501, 17.100 -16.000 -7.000, 17.135 -16.000 -6.001, 15.004 -16.000 -12.176, 7.474 15.100 -60.554, 7.540 15.100 -60.374, 7.602 15.100 -60.193, 7.474 17.700 -60.554, 3.142 17.900 -60.175, 3.142 17.700 -60.175, -3.219 17.900 -17.836, -8.000 17.900 -59.200, -3.892 17.900 -58.624, -3.524 17.900 -59.592, 4.354 17.900 -37.792, 3.473 17.900 -55.715, 2.847 17.900 -54.890, 2.031 17.900 -54.254, -3.999 17.900 -57.594, 1.078 17.900 -53.848, 0.053 17.900 -53.700, -0.975 17.900 -53.821, -1.938 17.900 -54.201, -2.771 17.900 -54.816, -3.837 17.900 -56.571, -3.419 17.900 -55.624, 3.611 17.900 -59.421, 3.902 17.900 -58.582, 4.000 17.900 -57.700, 3.866 17.900 -56.673, 6.526 17.900 -15.782, -3.991 17.900 -13.375, -4.095 17.900 -14.565, 6.096 17.900 0.420, 3.864 17.900 -1.035, 4.000 17.900 0.000, 3.864 17.900 1.035, 3.464 17.900 2.000, -3.864 17.900 1.035, 2.828 17.900 2.828, 19.151 17.900 -5.946, 3.464 17.900 -2.000, -3.991 17.900 -13.375, 17.178 17.900 -15.227, 2.000 17.900 -3.464, 2.828 17.900 -2.828, -1.035 17.900 3.864, -2.000 17.900 3.464, -3.464 17.900 -2.000, -2.828 17.900 -2.828, -2.000 17.900 -3.464, -3.864 17.900 -1.035, -0.000 17.900 4.000, -1.035 17.900 -3.864, -4.000 17.900 0.000, 1.035 17.900 3.864, -0.000 17.900 -4.000, 1.035 17.900 -3.864, 2.000 17.900 3.464, -2.828 17.900 2.828, -3.464 17.900 2.000, 8.000 14.900 -57.700, 7.799 14.900 -55.920, 7.775 15.100 -55.816, 7.208 14.900 -54.229, 6.255 14.900 -52.712, 4.988 14.900 -51.445, 3.471 14.900 -50.492, 2.982 15.100 -50.276, 1.780 14.900 -49.901, 0.000 14.900 -49.700, -1.780 14.900 -49.901, -3.471 14.900 -50.492, -4.988 14.900 -51.445, -5.777 15.100 -52.166, -6.255 14.900 -52.712, -6.918 15.100 -53.683, -7.208 14.900 -54.229, -7.799 14.900 -55.920, -7.670 17.700 -55.425, -7.905 17.700 -56.471, -8.000 14.900 -57.700, -7.998 17.700 -57.538, -7.948 15.100 -58.608, -7.948 17.700 -58.608, -7.799 14.900 -59.480, -7.923 15.100 -58.811, -7.892 15.100 -59.012, -7.856 15.100 -59.213, -7.856 17.700 -59.213, -7.208 14.900 -61.171, -7.287 17.700 -61.001, -6.255 14.900 -62.688, -6.317 17.700 -62.608, -4.988 14.900 -63.955, -5.000 17.700 -63.945, -3.471 14.900 -64.908, -3.407 17.700 -64.938, -1.780 14.900 -65.499, -1.627 17.700 -65.533, -0.000 14.900 -65.700, 0.242 17.700 -65.696, 1.780 14.900 -65.499, 2.099 17.700 -65.420, 3.471 14.900 -64.908, 3.840 17.700 -64.718, 4.988 14.900 -63.955, 5.369 17.700 -63.631, 6.255 14.900 -62.688, 6.603 17.700 -62.216, 7.208 14.900 -61.171, 7.799 14.900 -59.480, 7.776 17.700 -59.580, 7.868 17.700 -59.145, 7.993 15.100 -58.036, -2.828 14.900 -2.828, -2.000 14.900 -3.464, -3.464 14.900 -2.000, -1.035 14.900 -3.864, -3.864 14.900 -1.035, -0.000 14.900 -4.000, -4.000 14.900 0.000, -3.864 14.900 1.035, 1.035 14.900 -3.864, -3.464 14.900 2.000, 2.000 14.900 -3.464, -2.828 14.900 2.828, 2.828 14.900 -2.828, -2.000 14.900 3.464, 3.464 14.900 -2.000, 3.864 14.900 -1.035, -1.035 14.900 3.864, 4.000 14.900 0.000, -0.000 14.900 4.000, 1.035 14.900 3.864, 3.864 14.900 1.035, 2.000 14.900 3.464, 3.464 14.900 2.000, 2.828 14.900 2.828, -8.000 15.100 -59.200, -6.047 16.500 -36.883, -4.095 15.100 -14.565, 16.998 -16.000 -6.408, 17.100 -16.000 -6.000, 17.125 -16.000 -5.662, 17.137 -16.000 -5.667, 16.371 -16.000 -12.224, 2.326 17.700 -60.954, -0.907 17.700 -61.596, 0.217 17.700 -61.694, -0.191 17.700 -62.454, 1.324 17.700 -61.474, 2.099 17.700 -65.420, -2.856 17.700 -60.501, -1.960 17.700 -61.187, -7.287 17.700 -61.001, -7.856 17.700 -59.213, -3.524 17.700 -59.592, 4.000 14.900 -57.700, 3.864 14.900 -56.665, 3.464 14.900 -55.700, 2.828 14.900 -54.872, 2.000 14.900 -54.236, 1.035 14.900 -53.836, -0.000 14.900 -53.700, -1.035 14.900 -53.836, -2.000 14.900 -54.236, -2.828 14.900 -54.872, -3.464 14.900 -55.700, -3.864 14.900 -56.665, -4.000 14.900 -57.700, -3.864 14.900 -58.735, -3.464 14.900 -59.700, -2.828 14.900 -60.528, -2.000 14.900 -61.164, -1.035 14.900 -61.564, -0.000 14.900 -61.700, 1.035 14.900 -61.564, 2.000 14.900 -61.164, 2.828 14.900 -60.528, 3.464 14.900 -59.700, 3.864 14.900 -58.735, -7.856 15.100 -59.213, -6.255 14.900 -62.688, 6.255 14.900 -62.688, -3.471 14.900 -50.492, 3.471 14.900 -50.492, 17.122 -16.000 -5.833, -4.664 25.250 -15.302, -7.000 25.250 -42.000, -4.664 18.000 -15.302, -7.000 18.000 -42.000, 5.253 18.000 -43.072, 7.589 18.000 -16.374, 7.589 25.250 -16.374, 5.253 25.250 -43.072 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 3, 1, 0, -1, 4, 3, 0, -1, 5, 3, 4, -1, 6, 5, 4, -1, 7, 5, 6, -1, 8, 7, 6, -1, 9, 8, 10, -1, 9, 7, 8, -1, 11, 9, 10, -1, 12, 9, 11, -1, 13, 12, 11, -1, 14, 15, 13, -1, 16, 17, 14, -1, 18, 17, 16, -1, 19, 18, 16, -1, 20, 18, 19, -1, 21, 20, 19, -1, 22, 20, 21, -1, 23, 22, 21, -1, 24, 22, 23, -1, 25, 24, 23, -1, 26, 24, 25, -1, 26, 25, 27, -1, 28, 26, 27, -1, 29, 26, 28, -1, 30, 29, 28, -1, 31, 29, 30, -1, 32, 31, 30, -1, 33, 32, 34, -1, 33, 31, 32, -1, 35, 33, 34, -1, 36, 33, 35, -1, 37, 36, 35, -1, 38, 36, 37, -1, 38, 37, 39, -1, 40, 39, 37, -1, 41, 42, 40, -1, 43, 42, 41, -1, 44, 45, 43, -1, 46, 47, 44, -1, 48, 47, 46, -1, 49, 47, 48, -1, 50, 49, 48, -1, 51, 49, 50, -1, 52, 51, 50, -1, 53, 51, 52, -1, 54, 53, 52, -1, 55, 54, 56, -1, 55, 53, 54, -1, 57, 55, 56, -1, 58, 55, 57, -1, 59, 58, 57, -1, 60, 58, 59, -1, 61, 60, 59, -1, 62, 60, 61, -1, 63, 62, 61, -1, 1, 62, 63, -1, 1, 63, 2, -1, 42, 39, 40, -1, 45, 42, 43, -1, 47, 45, 44, -1, 15, 12, 13, -1, 17, 15, 14, -1, 64, 65, 66, -1, 64, 67, 65, -1, 68, 69, 70, -1, 68, 71, 69, -1, 72, 73, 74, -1, 72, 75, 73, -1, 76, 68, 70, -1, 77, 73, 75, -1, 78, 79, 68, -1, 78, 80, 81, -1, 78, 81, 82, -1, 83, 73, 77, -1, 80, 68, 76, -1, 78, 68, 80, -1, 84, 73, 83, -1, 85, 86, 64, -1, 85, 64, 66, -1, 87, 85, 78, -1, 88, 73, 84, -1, 87, 78, 82, -1, 87, 82, 89, -1, 87, 89, 90, -1, 87, 90, 91, -1, 87, 91, 92, -1, 93, 94, 95, -1, 87, 92, 96, -1, 93, 95, 71, -1, 97, 98, 67, -1, 97, 67, 99, -1, 64, 99, 67, -1, 87, 86, 85, -1, 100, 73, 101, -1, 101, 73, 88, -1, 101, 88, 102, -1, 103, 100, 101, -1, 71, 102, 104, -1, 71, 104, 69, -1, 71, 95, 101, -1, 71, 101, 102, -1, 65, 67, 105, -1, 41, 40, 106, -1, 43, 41, 106, -1, 44, 43, 107, -1, 43, 106, 107, -1, 40, 37, 108, -1, 106, 40, 108, -1, 46, 44, 109, -1, 48, 46, 109, -1, 44, 107, 109, -1, 35, 34, 110, -1, 37, 35, 110, -1, 108, 37, 110, -1, 34, 32, 111, -1, 110, 34, 111, -1, 50, 48, 112, -1, 48, 109, 112, -1, 30, 28, 113, -1, 32, 30, 113, -1, 111, 32, 113, -1, 50, 112, 114, -1, 52, 50, 114, -1, 113, 28, 115, -1, 28, 27, 115, -1, 52, 114, 116, -1, 54, 52, 116, -1, 54, 116, 56, -1, 115, 27, 117, -1, 56, 116, 118, -1, 117, 27, 25, -1, 56, 118, 57, -1, 57, 118, 119, -1, 120, 117, 23, -1, 117, 25, 23, -1, 57, 119, 59, -1, 121, 120, 21, -1, 120, 23, 21, -1, 119, 122, 61, -1, 59, 119, 61, -1, 121, 21, 19, -1, 122, 123, 63, -1, 61, 122, 63, -1, 124, 121, 16, -1, 121, 19, 16, -1, 63, 123, 2, -1, 125, 124, 14, -1, 124, 16, 14, -1, 123, 126, 0, -1, 2, 123, 0, -1, 127, 125, 13, -1, 125, 14, 13, -1, 126, 128, 4, -1, 0, 126, 4, -1, 127, 13, 11, -1, 4, 128, 6, -1, 129, 127, 10, -1, 127, 11, 10, -1, 128, 129, 8, -1, 129, 10, 8, -1, 6, 128, 8, -1, 18, 130, 131, -1, 132, 18, 131, -1, 133, 134, 17, -1, 133, 132, 134, -1, 133, 17, 18, -1, 133, 18, 132, -1, 36, 38, 135, -1, 33, 36, 135, -1, 31, 33, 135, -1, 29, 31, 135, -1, 130, 24, 26, -1, 130, 26, 29, -1, 130, 29, 135, -1, 22, 24, 130, -1, 20, 22, 130, -1, 18, 20, 130, -1, 135, 38, 136, -1, 38, 137, 136, -1, 39, 138, 139, -1, 138, 137, 139, -1, 38, 39, 139, -1, 137, 38, 139, -1, 140, 141, 93, -1, 140, 93, 71, -1, 142, 71, 68, -1, 142, 140, 71, -1, 68, 79, 143, -1, 68, 143, 142, -1, 144, 143, 79, -1, 144, 79, 78, -1, 145, 78, 85, -1, 145, 144, 78, -1, 85, 146, 145, -1, 85, 66, 146, -1, 146, 66, 65, -1, 147, 146, 65, -1, 105, 148, 147, -1, 65, 105, 147, -1, 105, 149, 148, -1, 105, 67, 149, -1, 98, 150, 149, -1, 67, 98, 149, -1, 151, 152, 153, -1, 151, 97, 152, -1, 151, 98, 97, -1, 151, 150, 98, -1, 151, 154, 150, -1, 151, 153, 154, -1, 155, 156, 152, -1, 99, 152, 97, -1, 99, 155, 152, -1, 155, 157, 156, -1, 158, 73, 100, -1, 158, 74, 73, -1, 158, 157, 74, -1, 158, 100, 156, -1, 158, 156, 157, -1, 159, 160, 161, -1, 159, 162, 160, -1, 163, 162, 159, -1, 164, 165, 166, -1, 167, 165, 164, -1, 168, 164, 169, -1, 168, 167, 164, -1, 165, 162, 163, -1, 170, 171, 166, -1, 170, 163, 171, -1, 170, 165, 163, -1, 170, 166, 165, -1, 172, 168, 173, -1, 168, 169, 173, -1, 134, 174, 175, -1, 132, 174, 134, -1, 103, 176, 177, -1, 176, 178, 177, -1, 178, 179, 177, -1, 179, 156, 177, -1, 156, 100, 177, -1, 100, 103, 177, -1, 180, 176, 103, -1, 180, 103, 101, -1, 181, 101, 95, -1, 181, 180, 101, -1, 94, 182, 181, -1, 95, 94, 181, -1, 141, 182, 94, -1, 141, 94, 93, -1, 117, 183, 184, -1, 119, 118, 185, -1, 118, 186, 185, -1, 123, 187, 126, -1, 113, 115, 188, -1, 115, 184, 188, -1, 122, 119, 189, -1, 126, 187, 190, -1, 119, 185, 189, -1, 113, 188, 111, -1, 126, 190, 128, -1, 111, 188, 191, -1, 123, 122, 187, -1, 122, 189, 187, -1, 128, 190, 192, -1, 111, 191, 193, -1, 110, 111, 193, -1, 128, 192, 129, -1, 129, 192, 194, -1, 110, 193, 195, -1, 108, 110, 195, -1, 127, 129, 196, -1, 129, 194, 196, -1, 108, 195, 197, -1, 106, 108, 197, -1, 125, 127, 198, -1, 127, 196, 198, -1, 106, 197, 199, -1, 107, 106, 199, -1, 124, 125, 200, -1, 125, 198, 200, -1, 107, 199, 201, -1, 109, 107, 201, -1, 121, 124, 202, -1, 109, 201, 203, -1, 124, 200, 202, -1, 112, 109, 203, -1, 120, 121, 204, -1, 114, 112, 205, -1, 112, 203, 205, -1, 121, 202, 204, -1, 114, 205, 116, -1, 117, 120, 183, -1, 116, 205, 206, -1, 120, 204, 183, -1, 118, 116, 186, -1, 116, 206, 186, -1, 115, 117, 184, -1, 207, 208, 209, -1, 210, 207, 209, -1, 211, 212, 213, -1, 214, 215, 216, -1, 217, 214, 216, -1, 218, 219, 220, -1, 221, 217, 216, -1, 222, 218, 220, -1, 218, 222, 223, -1, 224, 221, 216, -1, 225, 226, 224, -1, 227, 228, 209, -1, 229, 226, 230, -1, 231, 209, 232, -1, 233, 234, 232, -1, 235, 233, 232, -1, 236, 235, 232, -1, 237, 236, 232, -1, 208, 237, 232, -1, 213, 231, 232, -1, 209, 208, 232, -1, 234, 213, 232, -1, 224, 238, 239, -1, 224, 226, 240, -1, 238, 224, 240, -1, 241, 242, 243, -1, 224, 239, 244, -1, 212, 241, 243, -1, 216, 215, 245, -1, 240, 226, 246, -1, 224, 244, 247, -1, 229, 230, 248, -1, 246, 226, 249, -1, 250, 218, 251, -1, 224, 247, 221, -1, 229, 241, 212, -1, 252, 253, 212, -1, 211, 252, 212, -1, 249, 226, 253, -1, 254, 251, 223, -1, 253, 226, 212, -1, 226, 229, 212, -1, 216, 245, 255, -1, 251, 218, 223, -1, 213, 256, 231, -1, 216, 255, 210, -1, 223, 228, 227, -1, 254, 223, 227, -1, 216, 210, 209, -1, 231, 227, 209, -1, 234, 211, 213, -1, 135, 136, 131, -1, 130, 135, 131, -1, 257, 258, 259, -1, 258, 260, 259, -1, 259, 260, 261, -1, 261, 260, 262, -1, 262, 263, 264, -1, 263, 265, 264, -1, 265, 261, 264, -1, 261, 262, 264, -1, 148, 149, 147, -1, 266, 267, 142, -1, 146, 147, 268, -1, 267, 140, 142, -1, 147, 149, 268, -1, 269, 270, 271, -1, 266, 142, 272, -1, 270, 273, 271, -1, 273, 270, 274, -1, 142, 143, 144, -1, 274, 270, 275, -1, 276, 277, 144, -1, 278, 276, 144, -1, 272, 142, 277, -1, 275, 270, 279, -1, 277, 142, 144, -1, 279, 270, 280, -1, 268, 281, 145, -1, 146, 268, 145, -1, 144, 145, 282, -1, 278, 144, 282, -1, 283, 278, 282, -1, 284, 283, 282, -1, 181, 182, 141, -1, 285, 284, 282, -1, 286, 285, 282, -1, 287, 286, 282, -1, 140, 181, 141, -1, 149, 150, 154, -1, 288, 149, 154, -1, 149, 288, 268, -1, 145, 281, 282, -1, 180, 270, 178, -1, 280, 270, 180, -1, 289, 280, 180, -1, 180, 178, 176, -1, 290, 289, 140, -1, 267, 290, 140, -1, 180, 181, 140, -1, 289, 180, 140, -1, 152, 156, 179, -1, 152, 179, 153, -1, 153, 179, 291, -1, 154, 153, 288, -1, 153, 291, 288, -1, 179, 292, 291, -1, 178, 270, 293, -1, 270, 269, 293, -1, 269, 292, 293, -1, 179, 178, 293, -1, 292, 179, 293, -1, 294, 295, 296, -1, 294, 296, 297, -1, 294, 297, 298, -1, 294, 298, 299, -1, 294, 299, 300, -1, 294, 300, 157, -1, 294, 155, 301, -1, 157, 300, 302, -1, 157, 302, 303, -1, 74, 303, 304, -1, 74, 157, 303, -1, 155, 305, 306, -1, 155, 306, 307, -1, 155, 307, 308, -1, 155, 308, 309, -1, 155, 309, 301, -1, 294, 157, 155, -1, 294, 301, 310, -1, 294, 310, 295, -1, 311, 160, 312, -1, 161, 160, 313, -1, 313, 160, 311, -1, 162, 314, 315, -1, 314, 316, 315, -1, 315, 312, 160, -1, 162, 315, 160, -1, 317, 318, 319, -1, 317, 320, 321, -1, 317, 319, 322, -1, 317, 321, 225, -1, 317, 322, 323, -1, 317, 323, 324, -1, 325, 326, 327, -1, 328, 325, 327, -1, 329, 328, 327, -1, 330, 329, 327, -1, 331, 330, 327, -1, 332, 331, 327, -1, 320, 332, 327, -1, 317, 324, 320, -1, 333, 334, 327, -1, 334, 321, 327, -1, 321, 320, 327, -1, 326, 333, 327, -1, 335, 336, 333, -1, 337, 335, 333, -1, 338, 337, 333, -1, 339, 338, 333, -1, 326, 339, 333, -1, 340, 341, 334, -1, 220, 340, 222, -1, 340, 334, 222, -1, 341, 321, 334, -1, 230, 226, 342, -1, 226, 225, 342, -1, 225, 321, 342, -1, 341, 230, 342, -1, 321, 341, 342, -1, 343, 344, 345, -1, 346, 344, 343, -1, 347, 346, 343, -1, 348, 346, 347, -1, 349, 348, 347, -1, 350, 348, 349, -1, 351, 350, 349, -1, 352, 350, 351, -1, 353, 350, 352, -1, 354, 353, 352, -1, 355, 356, 354, -1, 357, 356, 355, -1, 358, 357, 355, -1, 359, 357, 358, -1, 360, 357, 359, -1, 361, 360, 359, -1, 362, 363, 361, -1, 364, 363, 362, -1, 365, 364, 362, -1, 366, 364, 365, -1, 367, 364, 366, -1, 368, 367, 366, -1, 369, 370, 368, -1, 371, 265, 369, -1, 261, 265, 371, -1, 372, 261, 371, -1, 373, 372, 371, -1, 374, 372, 373, -1, 375, 374, 373, -1, 376, 257, 375, -1, 259, 374, 375, -1, 259, 375, 257, -1, 377, 376, 375, -1, 378, 377, 375, -1, 379, 380, 378, -1, 379, 378, 375, -1, 381, 379, 382, -1, 381, 380, 379, -1, 383, 381, 382, -1, 384, 381, 383, -1, 385, 384, 383, -1, 386, 384, 385, -1, 387, 386, 385, -1, 388, 386, 387, -1, 389, 388, 387, -1, 390, 389, 391, -1, 390, 388, 389, -1, 392, 390, 391, -1, 393, 390, 392, -1, 394, 393, 392, -1, 395, 393, 394, -1, 396, 395, 394, -1, 397, 395, 396, -1, 397, 396, 398, -1, 399, 397, 398, -1, 400, 397, 399, -1, 401, 400, 399, -1, 402, 400, 401, -1, 403, 402, 401, -1, 404, 312, 403, -1, 315, 402, 403, -1, 315, 403, 312, -1, 313, 311, 404, -1, 161, 313, 404, -1, 405, 159, 161, -1, 406, 161, 404, -1, 406, 405, 161, -1, 407, 405, 406, -1, 163, 407, 406, -1, 163, 406, 171, -1, 408, 171, 406, -1, 345, 409, 408, -1, 345, 408, 406, -1, 344, 409, 345, -1, 356, 353, 354, -1, 363, 360, 361, -1, 370, 367, 368, -1, 265, 370, 369, -1, 311, 312, 404, -1, 159, 405, 407, -1, 159, 407, 163, -1, 212, 243, 410, -1, 243, 411, 410, -1, 213, 410, 412, -1, 213, 212, 410, -1, 213, 413, 256, -1, 412, 413, 213, -1, 256, 413, 231, -1, 231, 413, 414, -1, 415, 231, 414, -1, 227, 231, 415, -1, 227, 416, 254, -1, 415, 416, 227, -1, 254, 416, 417, -1, 251, 254, 417, -1, 418, 250, 251, -1, 417, 418, 251, -1, 418, 218, 250, -1, 418, 419, 218, -1, 419, 420, 219, -1, 419, 219, 218, -1, 220, 421, 340, -1, 422, 219, 420, -1, 422, 220, 219, -1, 422, 420, 421, -1, 422, 421, 220, -1, 248, 230, 423, -1, 341, 424, 425, -1, 424, 423, 425, -1, 230, 341, 425, -1, 423, 230, 425, -1, 248, 423, 229, -1, 229, 423, 426, -1, 241, 426, 427, -1, 241, 229, 426, -1, 241, 428, 242, -1, 427, 428, 241, -1, 242, 428, 243, -1, 243, 428, 411, -1, 377, 378, 258, -1, 376, 377, 258, -1, 257, 376, 258, -1, 429, 260, 258, -1, 429, 258, 430, -1, 260, 429, 431, -1, 431, 429, 432, -1, 259, 261, 372, -1, 259, 372, 374, -1, 433, 434, 435, -1, 434, 436, 435, -1, 435, 436, 437, -1, 291, 438, 439, -1, 439, 438, 440, -1, 441, 439, 442, -1, 439, 440, 442, -1, 437, 436, 438, -1, 291, 443, 444, -1, 443, 437, 444, -1, 437, 438, 444, -1, 438, 291, 444, -1, 445, 442, 446, -1, 445, 441, 442, -1, 281, 447, 282, -1, 282, 447, 448, -1, 449, 450, 451, -1, 449, 451, 448, -1, 287, 282, 452, -1, 282, 448, 452, -1, 451, 287, 452, -1, 448, 451, 452, -1, 453, 277, 454, -1, 454, 277, 276, -1, 454, 276, 455, -1, 455, 276, 278, -1, 455, 278, 456, -1, 456, 278, 283, -1, 456, 283, 457, -1, 457, 283, 284, -1, 457, 284, 458, -1, 458, 284, 285, -1, 458, 285, 459, -1, 459, 285, 286, -1, 459, 286, 460, -1, 287, 451, 461, -1, 460, 287, 461, -1, 461, 451, 462, -1, 461, 462, 463, -1, 463, 462, 464, -1, 463, 464, 465, -1, 465, 464, 466, -1, 465, 466, 467, -1, 467, 466, 468, -1, 467, 468, 469, -1, 469, 468, 470, -1, 469, 470, 471, -1, 471, 470, 472, -1, 471, 472, 473, -1, 473, 472, 474, -1, 473, 474, 475, -1, 475, 474, 476, -1, 475, 476, 477, -1, 477, 476, 478, -1, 271, 477, 478, -1, 477, 271, 479, -1, 479, 273, 480, -1, 480, 274, 481, -1, 481, 275, 482, -1, 483, 482, 279, -1, 482, 275, 279, -1, 484, 483, 280, -1, 483, 279, 280, -1, 485, 484, 289, -1, 484, 280, 289, -1, 486, 485, 290, -1, 485, 289, 290, -1, 487, 486, 267, -1, 486, 290, 267, -1, 488, 487, 266, -1, 487, 267, 266, -1, 489, 488, 272, -1, 488, 266, 272, -1, 453, 489, 277, -1, 489, 272, 277, -1, 479, 271, 273, -1, 480, 273, 274, -1, 481, 274, 275, -1, 460, 286, 287, -1, 490, 478, 491, -1, 490, 492, 478, -1, 493, 269, 271, -1, 493, 492, 269, -1, 493, 271, 478, -1, 493, 478, 492, -1, 494, 495, 496, -1, 497, 494, 496, -1, 498, 497, 496, -1, 499, 498, 496, -1, 500, 499, 496, -1, 292, 500, 496, -1, 501, 291, 496, -1, 502, 500, 292, -1, 503, 502, 292, -1, 504, 503, 269, -1, 503, 292, 269, -1, 505, 443, 291, -1, 506, 505, 291, -1, 507, 506, 291, -1, 508, 507, 291, -1, 501, 508, 291, -1, 291, 292, 496, -1, 509, 501, 496, -1, 495, 509, 496, -1, 316, 510, 511, -1, 402, 316, 511, -1, 315, 316, 402, -1, 512, 513, 514, -1, 512, 515, 513, -1, 512, 388, 390, -1, 512, 390, 393, -1, 512, 393, 516, -1, 512, 514, 388, -1, 512, 516, 515, -1, 517, 384, 386, -1, 518, 386, 388, -1, 518, 517, 386, -1, 432, 429, 519, -1, 432, 519, 384, -1, 432, 384, 517, -1, 514, 518, 388, -1, 515, 516, 397, -1, 510, 515, 397, -1, 511, 510, 397, -1, 520, 521, 522, -1, 521, 523, 522, -1, 524, 520, 525, -1, 520, 522, 525, -1, 526, 524, 527, -1, 524, 525, 527, -1, 528, 526, 529, -1, 526, 527, 529, -1, 530, 528, 531, -1, 528, 529, 531, -1, 532, 530, 533, -1, 530, 531, 533, -1, 534, 532, 535, -1, 532, 533, 535, -1, 536, 534, 537, -1, 534, 535, 537, -1, 538, 536, 539, -1, 536, 537, 539, -1, 540, 538, 541, -1, 538, 539, 541, -1, 542, 540, 543, -1, 540, 541, 543, -1, 542, 543, 544, -1, 545, 542, 544, -1, 545, 544, 546, -1, 547, 545, 546, -1, 547, 546, 431, -1, 432, 547, 431, -1, 547, 432, 548, -1, 549, 548, 517, -1, 549, 517, 550, -1, 550, 517, 518, -1, 550, 518, 551, -1, 551, 518, 514, -1, 551, 514, 552, -1, 552, 514, 513, -1, 552, 513, 553, -1, 553, 513, 515, -1, 553, 515, 554, -1, 554, 515, 510, -1, 554, 510, 555, -1, 316, 314, 556, -1, 555, 316, 556, -1, 556, 314, 557, -1, 556, 557, 558, -1, 558, 557, 559, -1, 558, 559, 521, -1, 521, 559, 523, -1, 548, 432, 517, -1, 555, 510, 316, -1, 421, 424, 340, -1, 424, 341, 340, -1, 560, 561, 549, -1, 387, 560, 549, -1, 389, 387, 550, -1, 387, 549, 550, -1, 561, 382, 548, -1, 549, 561, 548, -1, 391, 389, 551, -1, 392, 391, 551, -1, 389, 550, 551, -1, 379, 375, 547, -1, 382, 379, 547, -1, 548, 382, 547, -1, 394, 392, 552, -1, 392, 551, 552, -1, 375, 562, 545, -1, 547, 375, 545, -1, 396, 394, 553, -1, 394, 552, 553, -1, 545, 562, 542, -1, 371, 369, 542, -1, 562, 371, 542, -1, 542, 369, 540, -1, 369, 368, 540, -1, 396, 553, 554, -1, 398, 396, 554, -1, 398, 554, 399, -1, 540, 368, 538, -1, 399, 554, 555, -1, 538, 368, 366, -1, 399, 555, 401, -1, 401, 555, 556, -1, 536, 538, 563, -1, 538, 366, 563, -1, 401, 556, 403, -1, 534, 536, 564, -1, 536, 563, 564, -1, 556, 558, 404, -1, 403, 556, 404, -1, 534, 564, 361, -1, 558, 521, 406, -1, 404, 558, 406, -1, 532, 534, 359, -1, 534, 361, 359, -1, 406, 521, 345, -1, 530, 532, 565, -1, 532, 359, 565, -1, 521, 520, 566, -1, 345, 521, 566, -1, 528, 530, 355, -1, 530, 565, 355, -1, 520, 524, 567, -1, 566, 520, 567, -1, 528, 355, 354, -1, 567, 524, 349, -1, 526, 528, 352, -1, 528, 354, 352, -1, 524, 526, 351, -1, 526, 352, 351, -1, 349, 524, 351, -1, 411, 568, 569, -1, 568, 413, 569, -1, 413, 412, 569, -1, 412, 410, 569, -1, 410, 411, 569, -1, 570, 571, 416, -1, 416, 415, 572, -1, 415, 414, 572, -1, 414, 413, 572, -1, 413, 570, 572, -1, 570, 416, 572, -1, 416, 418, 417, -1, 420, 419, 418, -1, 424, 421, 423, -1, 423, 421, 573, -1, 428, 574, 411, -1, 411, 574, 575, -1, 576, 575, 420, -1, 575, 574, 420, -1, 573, 421, 420, -1, 574, 573, 420, -1, 428, 427, 577, -1, 427, 426, 577, -1, 426, 423, 577, -1, 423, 428, 577, -1, 578, 434, 579, -1, 580, 434, 433, -1, 579, 434, 580, -1, 581, 582, 436, -1, 581, 583, 582, -1, 434, 578, 581, -1, 434, 581, 436, -1, 492, 584, 585, -1, 586, 587, 585, -1, 588, 438, 436, -1, 588, 584, 438, -1, 588, 589, 590, -1, 588, 590, 591, -1, 592, 586, 585, -1, 588, 591, 593, -1, 588, 593, 594, -1, 588, 594, 595, -1, 588, 595, 596, -1, 588, 596, 597, -1, 598, 592, 585, -1, 588, 597, 584, -1, 588, 436, 589, -1, 599, 598, 585, -1, 597, 599, 585, -1, 436, 582, 600, -1, 436, 600, 601, -1, 436, 601, 602, -1, 436, 602, 603, -1, 436, 603, 589, -1, 584, 597, 585, -1, 604, 605, 440, -1, 604, 606, 605, -1, 604, 584, 606, -1, 604, 438, 584, -1, 604, 440, 438, -1, 607, 608, 609, -1, 607, 609, 610, -1, 607, 610, 611, -1, 449, 612, 490, -1, 607, 611, 613, -1, 607, 448, 614, -1, 607, 614, 615, -1, 607, 613, 448, -1, 490, 616, 492, -1, 617, 618, 619, -1, 617, 616, 618, -1, 620, 621, 449, -1, 622, 623, 490, -1, 624, 616, 490, -1, 624, 490, 623, -1, 625, 622, 490, -1, 626, 620, 449, -1, 627, 616, 624, -1, 628, 625, 490, -1, 629, 626, 449, -1, 630, 616, 627, -1, 612, 628, 490, -1, 631, 616, 630, -1, 614, 619, 615, -1, 618, 616, 631, -1, 614, 617, 619, -1, 448, 613, 632, -1, 448, 632, 629, -1, 448, 447, 614, -1, 448, 629, 449, -1, 449, 621, 633, -1, 449, 633, 634, -1, 607, 615, 608, -1, 449, 634, 612, -1, 635, 507, 636, -1, 636, 507, 637, -1, 636, 637, 638, -1, 638, 637, 501, -1, 638, 501, 639, -1, 639, 501, 509, -1, 639, 509, 640, -1, 640, 509, 495, -1, 640, 495, 641, -1, 641, 495, 642, -1, 641, 642, 643, -1, 643, 642, 497, -1, 643, 497, 644, -1, 644, 498, 645, -1, 645, 499, 646, -1, 646, 500, 647, -1, 647, 648, 649, -1, 649, 650, 651, -1, 651, 504, 652, -1, 504, 653, 652, -1, 652, 653, 654, -1, 652, 654, 655, -1, 655, 654, 656, -1, 657, 655, 658, -1, 655, 656, 658, -1, 655, 657, 659, -1, 657, 660, 659, -1, 660, 661, 659, -1, 661, 662, 659, -1, 662, 663, 659, -1, 664, 659, 665, -1, 659, 663, 665, -1, 666, 664, 667, -1, 664, 665, 667, -1, 668, 666, 669, -1, 666, 667, 669, -1, 668, 669, 670, -1, 670, 669, 671, -1, 670, 671, 672, -1, 672, 671, 673, -1, 672, 673, 674, -1, 674, 673, 675, -1, 674, 675, 676, -1, 676, 675, 677, -1, 676, 677, 678, -1, 678, 677, 679, -1, 678, 679, 680, -1, 680, 679, 681, -1, 680, 681, 682, -1, 682, 681, 683, -1, 682, 683, 684, -1, 684, 578, 579, -1, 684, 683, 581, -1, 578, 684, 581, -1, 684, 579, 685, -1, 579, 580, 685, -1, 580, 433, 685, -1, 433, 435, 686, -1, 685, 433, 686, -1, 685, 686, 687, -1, 685, 687, 443, -1, 443, 687, 437, -1, 443, 505, 635, -1, 505, 688, 635, -1, 685, 443, 635, -1, 635, 688, 507, -1, 644, 497, 498, -1, 645, 498, 499, -1, 646, 499, 500, -1, 647, 500, 648, -1, 649, 648, 650, -1, 651, 650, 504, -1, 687, 686, 435, -1, 437, 687, 435, -1, 449, 490, 491, -1, 449, 491, 450, -1, 491, 478, 476, -1, 491, 476, 474, -1, 491, 474, 472, -1, 491, 472, 470, -1, 468, 466, 450, -1, 470, 468, 450, -1, 491, 470, 450, -1, 450, 466, 464, -1, 450, 464, 462, -1, 450, 462, 451, -1, 689, 479, 480, -1, 690, 480, 481, -1, 690, 689, 480, -1, 691, 477, 479, -1, 691, 479, 689, -1, 692, 481, 482, -1, 692, 690, 481, -1, 693, 475, 477, -1, 693, 477, 691, -1, 694, 482, 483, -1, 694, 483, 484, -1, 694, 692, 482, -1, 695, 471, 473, -1, 695, 473, 475, -1, 695, 475, 693, -1, 696, 469, 471, -1, 696, 471, 695, -1, 697, 484, 485, -1, 697, 694, 484, -1, 698, 469, 696, -1, 699, 697, 485, -1, 486, 699, 485, -1, 467, 469, 698, -1, 700, 467, 698, -1, 701, 699, 486, -1, 487, 701, 486, -1, 465, 467, 700, -1, 702, 465, 700, -1, 703, 701, 487, -1, 488, 703, 487, -1, 488, 704, 703, -1, 463, 702, 705, -1, 463, 465, 702, -1, 489, 704, 488, -1, 489, 706, 704, -1, 461, 705, 707, -1, 461, 463, 705, -1, 453, 706, 489, -1, 460, 461, 707, -1, 459, 707, 708, -1, 459, 460, 707, -1, 454, 709, 706, -1, 454, 706, 453, -1, 458, 708, 710, -1, 458, 459, 708, -1, 455, 711, 709, -1, 455, 709, 454, -1, 457, 710, 712, -1, 457, 458, 710, -1, 456, 712, 711, -1, 456, 457, 712, -1, 456, 711, 455, -1, 658, 713, 657, -1, 658, 585, 713, -1, 653, 585, 658, -1, 606, 585, 653, -1, 714, 715, 606, -1, 714, 504, 715, -1, 714, 653, 504, -1, 714, 606, 653, -1, 575, 576, 716, -1, 576, 717, 716, -1, 717, 568, 716, -1, 568, 411, 716, -1, 411, 575, 716, -1, 571, 570, 718, -1, 570, 413, 718, -1, 413, 568, 718, -1, 568, 719, 718, -1, 719, 571, 718, -1, 576, 420, 416, -1, 420, 418, 416, -1, 571, 576, 416, -1, 574, 428, 720, -1, 428, 423, 720, -1, 423, 573, 720, -1, 573, 574, 720, -1, 681, 721, 583, -1, 681, 583, 683, -1, 683, 583, 581, -1, 722, 723, 724, -1, 723, 725, 724, -1, 673, 671, 724, -1, 675, 673, 724, -1, 726, 675, 724, -1, 671, 722, 724, -1, 725, 726, 724, -1, 669, 667, 727, -1, 671, 669, 728, -1, 669, 727, 728, -1, 729, 730, 731, -1, 667, 729, 731, -1, 727, 667, 731, -1, 671, 728, 722, -1, 679, 726, 725, -1, 679, 725, 721, -1, 679, 721, 681, -1, 603, 732, 733, -1, 603, 602, 732, -1, 589, 733, 734, -1, 589, 603, 733, -1, 590, 734, 735, -1, 590, 589, 734, -1, 591, 735, 736, -1, 591, 590, 735, -1, 593, 736, 737, -1, 593, 591, 736, -1, 594, 737, 738, -1, 594, 593, 737, -1, 595, 738, 739, -1, 595, 594, 738, -1, 596, 739, 740, -1, 596, 595, 739, -1, 597, 740, 741, -1, 597, 596, 740, -1, 599, 741, 742, -1, 599, 597, 741, -1, 598, 742, 743, -1, 598, 599, 742, -1, 592, 598, 743, -1, 592, 743, 744, -1, 586, 592, 744, -1, 586, 744, 745, -1, 587, 586, 745, -1, 587, 745, 731, -1, 746, 731, 745, -1, 727, 746, 747, -1, 748, 727, 747, -1, 728, 727, 748, -1, 749, 728, 748, -1, 722, 728, 749, -1, 750, 722, 749, -1, 723, 722, 750, -1, 751, 723, 750, -1, 725, 723, 751, -1, 752, 725, 751, -1, 721, 725, 752, -1, 753, 721, 752, -1, 754, 582, 583, -1, 754, 583, 753, -1, 600, 582, 754, -1, 755, 600, 754, -1, 601, 600, 755, -1, 732, 601, 755, -1, 602, 601, 732, -1, 727, 731, 746, -1, 583, 721, 753, -1, 713, 585, 730, -1, 756, 713, 730, -1, 587, 730, 585, -1, 731, 730, 587, -1, 634, 633, 700, -1, 615, 701, 703, -1, 615, 619, 701, -1, 709, 609, 706, -1, 612, 698, 696, -1, 612, 634, 698, -1, 608, 703, 704, -1, 610, 609, 709, -1, 608, 615, 703, -1, 695, 612, 696, -1, 711, 610, 709, -1, 628, 612, 695, -1, 609, 704, 706, -1, 609, 608, 704, -1, 611, 610, 711, -1, 625, 628, 695, -1, 625, 695, 693, -1, 712, 611, 711, -1, 613, 611, 712, -1, 622, 625, 693, -1, 622, 693, 691, -1, 632, 712, 710, -1, 632, 613, 712, -1, 623, 622, 691, -1, 623, 691, 689, -1, 629, 710, 708, -1, 629, 632, 710, -1, 624, 623, 689, -1, 624, 689, 690, -1, 626, 708, 707, -1, 626, 629, 708, -1, 627, 624, 690, -1, 627, 690, 692, -1, 620, 707, 705, -1, 630, 627, 692, -1, 620, 626, 707, -1, 630, 692, 694, -1, 621, 705, 702, -1, 631, 694, 697, -1, 631, 630, 694, -1, 621, 620, 705, -1, 699, 631, 697, -1, 633, 702, 700, -1, 618, 631, 699, -1, 633, 621, 702, -1, 619, 699, 701, -1, 619, 618, 699, -1, 634, 700, 698, -1, 747, 757, 668, -1, 748, 668, 670, -1, 748, 747, 668, -1, 746, 664, 757, -1, 746, 757, 747, -1, 749, 670, 672, -1, 749, 748, 670, -1, 745, 659, 664, -1, 745, 664, 746, -1, 744, 652, 655, -1, 744, 655, 659, -1, 744, 659, 745, -1, 750, 672, 674, -1, 750, 674, 676, -1, 750, 749, 672, -1, 743, 651, 652, -1, 743, 652, 744, -1, 751, 676, 678, -1, 751, 750, 676, -1, 742, 651, 743, -1, 752, 751, 678, -1, 680, 752, 678, -1, 649, 651, 742, -1, 741, 649, 742, -1, 753, 752, 680, -1, 758, 753, 680, -1, 647, 649, 741, -1, 740, 647, 741, -1, 754, 753, 758, -1, 684, 754, 758, -1, 684, 755, 754, -1, 759, 740, 739, -1, 759, 647, 740, -1, 685, 755, 684, -1, 685, 732, 755, -1, 645, 739, 738, -1, 645, 759, 739, -1, 635, 732, 685, -1, 644, 645, 738, -1, 643, 738, 737, -1, 643, 644, 738, -1, 636, 733, 732, -1, 636, 732, 635, -1, 760, 737, 736, -1, 760, 643, 737, -1, 638, 734, 733, -1, 638, 733, 636, -1, 640, 736, 735, -1, 640, 760, 736, -1, 639, 735, 734, -1, 639, 640, 735, -1, 639, 734, 638, -1, 654, 653, 658, -1, 656, 654, 658, -1, 713, 662, 661, -1, 713, 661, 660, -1, 713, 660, 657, -1, 568, 571, 719, -1, 576, 571, 761, -1, 568, 717, 761, -1, 717, 576, 761, -1, 571, 568, 761, -1, 762, 763, 764, -1, 763, 765, 764, -1, 765, 766, 767, -1, 764, 765, 767, -1, 767, 762, 764, -1, 768, 762, 767, -1, 768, 769, 763, -1, 768, 763, 762, -1, 765, 763, 769, -1, 766, 765, 769, -1, 768, 766, 769, -1, 767, 766, 768, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 5.793 4.625 70.250 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 5.793 4.625 -120.312 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 5.793 -90.656 -25.031 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 5.793 99.906 -25.031 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -89.488 4.625 -25.031 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 101.074 4.625 -25.031 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_SHOULDER_P.wrl000066400000000000000000002606471207742442300235610ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 36.000 32.500 33.997 center 0.000 0.000 0.000 #translation 2.000 -17.250 0.001 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -0.000 1.000 -4.000, 0.000 3.000 -4.000, -0.832 1.000 -3.913, -0.832 3.000 -3.913, -1.627 1.000 -3.654, -1.627 3.000 -3.654, -2.351 1.000 -3.236, -2.351 3.000 -3.236, -2.973 1.000 -2.677, -2.973 3.000 -2.677, -3.464 1.000 -2.000, -3.464 3.000 -2.000, -3.804 1.000 -1.236, -3.804 3.000 -1.236, -3.978 1.000 -0.418, -3.978 3.000 -0.418, -3.978 1.000 0.418, -3.978 3.000 0.418, -3.804 1.000 1.236, -3.804 3.000 1.236, -3.464 3.000 2.000, -3.464 1.000 2.000, -2.973 3.000 2.677, -2.973 1.000 2.677, -2.351 3.000 3.236, -2.351 1.000 3.236, -1.627 3.000 3.654, -1.627 1.000 3.654, -0.832 3.000 3.913, -0.832 1.000 3.913, -0.000 1.000 4.000, 0.000 3.000 4.000, 0.832 1.000 3.913, 0.832 3.000 3.913, 1.627 1.000 3.654, 1.627 3.000 3.654, 2.351 1.000 3.236, 2.351 3.000 3.236, 2.973 1.000 2.677, 2.973 3.000 2.677, 3.464 1.000 2.000, 3.464 3.000 2.000, 3.804 1.000 1.236, 3.804 3.000 1.236, 3.978 1.000 0.418, 3.978 3.000 0.418, 3.978 1.000 -0.418, 3.978 3.000 -0.418, 3.804 1.000 -1.236, 3.804 3.000 -1.236, 3.464 1.000 -2.000, 3.464 3.000 -2.000, 2.973 1.000 -2.677, 2.973 3.000 -2.677, 2.351 1.000 -3.236, 2.351 3.000 -3.236, 1.627 1.000 -3.654, 1.627 3.000 -3.654, 0.832 1.000 -3.913, 0.832 3.000 -3.913, -16.000 1.000 11.000, 12.000 1.000 11.000, -16.000 1.000 3.500, -16.000 1.000 -13.500, 12.000 1.000 -13.500, 12.000 3.000 11.000, -16.000 3.000 11.000, -16.000 3.000 3.500, -16.000 3.000 -13.500, 12.000 3.000 -13.500, -18.000 1.000 11.000, -18.000 1.000 3.500, -18.000 1.000 -13.500, 14.000 1.000 11.000, 14.000 1.000 -13.500, -16.000 24.361 -16.419, -16.000 23.246 -16.176, -16.000 3.000 -16.500, -16.000 22.177 -15.777, -16.000 21.175 -15.230, -16.000 20.261 -14.546, -16.000 19.454 -13.739, -16.000 18.770 -12.825, -16.000 18.223 -11.823, -16.000 19.298 -3.446, -16.000 20.045 -2.648, -16.000 20.420 3.500, -16.000 20.894 -1.959, -16.000 21.828 -1.392, -16.000 22.831 -0.958, -16.000 23.884 -0.665, -16.000 24.968 -0.518, -16.000 26.061 -0.520, -16.000 27.143 -0.671, -16.000 28.195 -0.968, -16.000 17.824 -10.754, -16.000 17.581 -9.639, -16.000 25.500 -16.500, -16.000 29.197 -1.405, -16.000 16.098 -6.500, -16.000 17.500 -8.500, -16.000 17.575 -7.409, -16.000 17.797 -6.339, -16.000 18.164 -5.309, -16.000 18.667 -4.339, 12.000 3.000 -16.500, 12.000 23.028 -16.108, 12.000 24.249 -16.402, 12.000 21.868 -15.628, 12.000 20.798 -14.972, 12.000 19.843 -14.157, 12.000 19.028 -13.202, 12.000 18.372 -12.132, 12.000 17.892 -10.972, 12.000 17.598 -9.751, 12.000 17.500 -8.500, 12.000 7.000 11.000, 12.000 22.394 -1.127, 12.000 21.351 -1.660, 12.000 23.504 -0.753, 12.000 24.656 -0.545, 12.000 25.827 -0.507, 12.000 25.500 -16.500, 12.000 26.990 -0.640, 12.000 28.122 -0.942, 12.000 29.197 -1.405, 12.000 16.098 -2.750, 12.000 17.586 -7.332, 12.000 17.841 -6.189, 12.000 18.260 -5.096, 12.000 18.835 -4.075, 12.000 19.552 -3.150, 12.000 20.397 -2.339, -18.000 3.000 3.500, -18.000 3.000 11.000, -18.000 3.000 -13.500, -18.000 1.000 -16.500, -16.000 1.000 -16.500, 14.000 3.000 11.000, 12.000 1.000 -16.500, 14.000 3.000 -13.500, 14.000 1.000 -16.500, -18.000 20.420 3.500, -18.000 25.500 -16.500, -18.000 3.000 -16.500, -16.000 25.833 -16.493, -16.000 26.000 -16.484, -16.000 26.000 -13.475, -16.000 25.667 -16.498, -16.000 25.500 -16.500, -16.000 24.361 -16.419, -16.000 25.073 -13.482, -16.000 23.246 -16.176, -16.000 24.161 -13.317, -16.000 22.177 -15.777, -16.000 21.175 -15.230, -16.000 23.295 -12.987, -16.000 22.504 -12.503, -16.000 20.261 -14.546, -16.000 21.817 -11.881, -16.000 19.454 -13.739, -16.000 21.256 -11.143, -16.000 18.770 -12.825, -16.000 28.200 -4.292, -16.000 28.904 -4.838, -16.000 29.500 -1.575, -16.000 29.500 -5.500, -16.000 27.410 -3.879, -16.000 29.197 -1.405, -16.000 18.223 -11.823, -16.000 20.841 -10.314, -16.000 26.560 -3.614, -16.000 28.195 -0.968, -16.000 17.824 -10.754, -16.000 20.586 -9.423, -16.000 27.143 -0.671, -16.000 20.500 -8.500, -16.000 17.581 -9.639, -16.000 25.676 -3.503, -16.000 26.061 -0.520, -16.000 17.500 -8.500, -16.000 24.786 -3.551, -16.000 24.968 -0.518, -16.000 20.579 -7.613, -16.000 17.575 -7.409, -16.000 23.919 -3.757, -16.000 23.884 -0.665, -16.000 20.815 -6.753, -16.000 17.797 -6.339, -16.000 22.831 -0.958, -16.000 21.199 -5.949, -16.000 18.164 -5.309, -16.000 23.102 -4.113, -16.000 21.828 -1.392, -16.000 18.667 -4.339, -16.000 22.361 -4.608, -16.000 20.894 -1.959, -16.000 21.720 -5.227, -16.000 19.298 -3.446, -16.000 20.045 -2.648, -16.000 20.420 3.500, -16.000 29.197 -1.405, -18.000 29.197 -1.405, -18.000 20.420 3.500, 14.000 7.000 11.000, 14.000 25.500 -16.500, 14.000 3.000 -16.500, 12.000 29.500 -1.575, 12.000 27.988 -5.368, 12.000 27.230 -4.893, 12.000 17.500 -8.500, 12.000 21.500 -8.500, 12.000 17.598 -9.751, 12.000 29.500 -6.000, 12.000 24.656 -0.545, 12.000 25.495 -4.500, 12.000 24.606 -4.601, 12.000 25.827 -0.507, 12.000 17.586 -7.332, 12.000 29.197 -1.405, 12.000 21.600 -7.611, 12.000 18.372 -12.132, 12.000 21.854 -10.146, 12.000 22.282 -10.876, 12.000 19.028 -13.202, 12.000 26.000 -12.469, 12.000 26.000 -16.484, 12.000 25.833 -16.493, 12.000 25.667 -16.498, 12.000 23.504 -0.753, 12.000 17.841 -6.189, 12.000 21.896 -6.766, 12.000 25.154 -12.485, 12.000 25.500 -16.500, 12.000 24.249 -16.402, 12.000 22.394 -1.127, 12.000 23.761 -4.898, 12.000 24.323 -12.323, 12.000 23.028 -16.108, 12.000 21.868 -15.628, 12.000 28.122 -0.942, 12.000 26.385 -4.599, 12.000 18.260 -5.096, 12.000 17.892 -10.972, 12.000 21.590 -9.342, 12.000 23.545 -11.990, 12.000 20.798 -14.972, 12.000 21.351 -1.660, 12.000 23.004 -5.374, 12.000 18.835 -4.075, 12.000 22.371 -6.008, 12.000 20.397 -2.339, 12.000 22.854 -11.500, 12.000 19.552 -3.150, 12.000 19.843 -14.157, 12.000 28.622 -6.000, 12.000 26.990 -0.640, 12.000 7.000 11.000, 14.000 7.000 11.000, 14.000 29.197 -1.405, 12.000 29.197 -1.405, -18.000 23.246 -16.176, -18.000 24.361 -16.419, -18.000 22.177 -15.777, -18.000 21.175 -15.230, -18.000 20.261 -14.546, -18.000 19.454 -13.739, -18.000 18.770 -12.825, -18.000 18.223 -11.823, -18.000 20.045 -2.648, -18.000 19.298 -3.446, -18.000 20.894 -1.959, -18.000 21.828 -1.392, -18.000 22.831 -0.958, -18.000 23.884 -0.665, -18.000 24.968 -0.518, -18.000 26.061 -0.520, -18.000 27.143 -0.671, -18.000 28.195 -0.968, -18.000 17.824 -10.754, -18.000 17.581 -9.639, -18.000 29.197 -1.405, -18.000 16.098 -6.500, -18.000 17.500 -8.500, -18.000 17.575 -7.409, -18.000 17.797 -6.339, -18.000 18.164 -5.309, -18.000 18.667 -4.339, 14.000 24.249 -16.402, 14.000 23.028 -16.108, 14.000 21.868 -15.628, 14.000 20.798 -14.972, 14.000 19.843 -14.157, 14.000 19.028 -13.202, 14.000 18.372 -12.132, 14.000 17.892 -10.972, 14.000 17.598 -9.751, 14.000 17.500 -8.500, 14.000 21.351 -1.660, 14.000 22.394 -1.127, 14.000 23.504 -0.753, 14.000 24.656 -0.545, 14.000 25.827 -0.507, 14.000 26.990 -0.640, 14.000 28.122 -0.942, 14.000 29.197 -1.405, 14.000 17.586 -7.332, 14.000 16.098 -2.750, 14.000 17.841 -6.189, 14.000 18.260 -5.096, 14.000 18.835 -4.075, 14.000 19.552 -3.150, 14.000 20.397 -2.339, -16.000 26.000 -16.500, -18.000 25.500 -16.500, -18.000 26.000 -16.500, -16.000 30.976 -2.668, -16.000 30.130 -1.976, -16.000 28.138 -16.052, -16.000 29.143 -15.622, -16.000 27.686 -12.997, -16.000 28.440 -12.544, -16.000 32.824 -11.719, -16.000 33.194 -10.690, -16.000 30.078 -10.509, -16.000 32.317 -12.687, -16.000 29.655 -11.281, -16.000 27.084 -16.342, -16.000 26.864 -13.310, -16.000 33.421 -9.621, -16.000 30.360 -9.676, -16.000 33.500 -8.530, -16.000 30.491 -8.806, -16.000 33.429 -7.439, -16.000 33.211 -6.368, -16.000 30.467 -7.926, -16.000 32.848 -5.336, -16.000 30.290 -7.065, -16.000 30.932 -14.373, -16.000 31.682 -13.578, -16.000 29.104 -11.966, -16.000 32.348 -4.364, -16.000 29.964 -6.248, -16.000 30.080 -15.059, -16.000 31.720 -3.469, -16.000 20.579 -7.613, -16.000 20.500 -8.500, -18.000 20.579 -7.613, -18.000 20.500 -8.500, -16.000 20.815 -6.753, -18.000 20.815 -6.753, -16.000 21.199 -5.949, -18.000 21.199 -5.949, -16.000 21.720 -5.227, -18.000 21.720 -5.227, -16.000 22.361 -4.608, -18.000 22.361 -4.608, -16.000 23.102 -4.113, -18.000 23.102 -4.113, -18.000 23.919 -3.757, -16.000 23.919 -3.757, -16.000 24.786 -3.551, -18.000 24.786 -3.551, -18.000 25.676 -3.503, -16.000 25.676 -3.503, -16.000 26.560 -3.614, -18.000 26.560 -3.614, -16.000 27.410 -3.879, -18.000 27.410 -3.879, -18.000 28.200 -4.292, -16.000 28.200 -4.292, -16.000 28.904 -4.838, -18.000 28.904 -4.838, -18.000 29.500 -5.500, -16.000 29.500 -5.500, -16.000 29.964 -6.248, -18.000 29.964 -6.248, -16.000 30.290 -7.065, -18.000 30.290 -7.065, -18.000 30.467 -7.926, -16.000 30.467 -7.926, -16.000 30.491 -8.806, -18.000 30.491 -8.806, -16.000 30.360 -9.676, -18.000 30.360 -9.676, -16.000 30.078 -10.509, -18.000 30.078 -10.509, -16.000 29.655 -11.281, -18.000 29.655 -11.281, -16.000 29.104 -11.966, -18.000 29.104 -11.966, -16.000 28.440 -12.544, -18.000 28.440 -12.544, -16.000 27.686 -12.997, -18.000 27.686 -12.997, -16.000 26.864 -13.310, -18.000 26.864 -13.310, -16.000 26.000 -13.475, -18.000 26.000 -13.475, -16.000 25.073 -13.482, -18.000 25.073 -13.482, -16.000 24.161 -13.317, -18.000 24.161 -13.317, -16.000 23.295 -12.987, -18.000 23.295 -12.987, -16.000 22.504 -12.503, -18.000 22.504 -12.503, -16.000 21.817 -11.881, -18.000 21.817 -11.881, -16.000 21.256 -11.143, -18.000 21.256 -11.143, -18.000 20.841 -10.314, -16.000 20.841 -10.314, -18.000 20.586 -9.423, -16.000 20.586 -9.423, -16.000 32.824 -11.719, -18.000 33.194 -10.690, -16.000 33.194 -10.690, -18.000 32.824 -11.719, -16.000 30.130 -1.976, -16.000 32.317 -12.687, -18.000 30.130 -1.976, -18.000 32.317 -12.687, -16.000 31.682 -13.578, -16.000 30.976 -2.668, -18.000 31.682 -13.578, -18.000 30.976 -2.668, -18.000 30.932 -14.373, -16.000 30.932 -14.373, -18.000 31.720 -3.469, -16.000 31.720 -3.469, -18.000 30.080 -15.059, -16.000 32.348 -4.364, -16.000 30.080 -15.059, -18.000 32.348 -4.364, -18.000 29.143 -15.622, -16.000 32.848 -5.336, -16.000 29.143 -15.622, -18.000 32.848 -5.336, -16.000 28.138 -16.052, -18.000 28.138 -16.052, -16.000 33.211 -6.368, -16.000 27.084 -16.342, -18.000 33.211 -6.368, -18.000 27.084 -16.342, -18.000 33.429 -7.439, -16.000 33.429 -7.439, -18.000 26.000 -16.484, -16.000 26.000 -16.484, -16.000 33.500 -8.530, -18.000 33.500 -8.530, -18.000 33.421 -9.621, -16.000 33.421 -9.621, 14.000 26.000 -16.500, 14.000 25.500 -16.500, 12.000 25.500 -16.500, 12.000 26.000 -16.500, 12.000 26.000 -16.500, 12.000 29.749 -8.945, 12.000 33.498 -8.337, 12.000 33.430 -9.557, 12.000 33.176 -10.752, 12.000 29.207 -10.003, 12.000 32.744 -11.895, 12.000 29.497 -8.344, 12.000 33.380 -7.121, 12.000 28.808 -10.749, 12.000 26.824 -12.274, 12.000 28.380 -15.964, 12.000 27.210 -16.315, 12.000 27.589 -11.911, 12.000 29.375 -7.506, 12.000 33.078 -5.937, 12.000 29.079 -6.713, 12.000 31.956 -3.775, 12.000 32.600 -4.813, 12.000 28.260 -11.395, 12.000 31.386 -13.918, 12.000 30.493 -14.751, 12.000 31.161 -2.847, 12.000 30.234 -2.051, 12.000 32.143 -12.958, 12.000 29.483 -15.438, 12.000 29.440 -9.189, 14.000 21.600 -7.611, 14.000 21.500 -8.500, 12.000 21.600 -7.611, 12.000 21.500 -8.500, 14.000 21.896 -6.766, 12.000 21.896 -6.766, 14.000 22.371 -6.008, 12.000 22.371 -6.008, 14.000 23.004 -5.374, 12.000 23.004 -5.374, 14.000 23.761 -4.898, 12.000 23.761 -4.898, 14.000 24.606 -4.601, 12.000 24.606 -4.601, 14.000 25.495 -4.500, 12.000 25.495 -4.500, 14.000 26.385 -4.599, 12.000 26.385 -4.599, 14.000 27.230 -4.893, 12.000 27.230 -4.893, 12.000 27.988 -5.368, 14.000 27.988 -5.368, 14.000 28.622 -6.000, 12.000 28.622 -6.000, 12.000 29.079 -6.713, 14.000 29.079 -6.713, 12.000 29.375 -7.506, 14.000 29.375 -7.506, 12.000 29.497 -8.344, 14.000 29.497 -8.344, 12.000 29.440 -9.189, 14.000 29.440 -9.189, 12.000 29.207 -10.003, 14.000 29.207 -10.003, 12.000 28.808 -10.749, 14.000 28.808 -10.749, 12.000 28.260 -11.395, 14.000 28.260 -11.395, 12.000 27.589 -11.911, 14.000 27.589 -11.911, 12.000 26.824 -12.274, 14.000 26.824 -12.274, 12.000 26.000 -12.469, 14.000 26.000 -12.469, 14.000 25.154 -12.485, 12.000 25.154 -12.485, 14.000 24.323 -12.323, 12.000 24.323 -12.323, 14.000 23.545 -11.990, 12.000 23.545 -11.990, 14.000 22.854 -11.500, 12.000 22.854 -11.500, 14.000 22.282 -10.876, 12.000 22.282 -10.876, 14.000 21.854 -10.146, 12.000 21.854 -10.146, 14.000 21.590 -9.342, 12.000 21.590 -9.342, 12.000 32.744 -11.895, 12.000 33.176 -10.752, 14.000 32.744 -11.895, 12.000 32.143 -12.958, 14.000 32.143 -12.958, 14.000 30.234 -2.051, 14.000 31.386 -13.918, 12.000 30.234 -2.051, 12.000 31.386 -13.918, 14.000 30.493 -14.751, 12.000 31.161 -2.847, 14.000 31.161 -2.847, 12.000 30.493 -14.751, 12.000 29.483 -15.438, 14.000 29.483 -15.438, 12.000 31.956 -3.775, 14.000 31.956 -3.775, 14.000 28.380 -15.964, 12.000 28.380 -15.964, 12.000 32.600 -4.813, 14.000 32.600 -4.813, 14.000 27.210 -16.315, 12.000 27.210 -16.315, 14.000 33.078 -5.937, 12.000 26.000 -16.484, 12.000 33.078 -5.937, 14.000 26.000 -16.484, 12.000 33.380 -7.121, 14.000 33.380 -7.121, 12.000 33.498 -8.337, 14.000 33.498 -8.337, 14.000 33.430 -9.557, 12.000 33.430 -9.557, 14.000 33.176 -10.752, -18.000 25.833 -16.493, -18.000 25.667 -16.498, -18.000 24.361 -16.419, -18.000 23.246 -16.176, -18.000 22.177 -15.777, -18.000 21.175 -15.230, -18.000 20.261 -14.546, -18.000 19.454 -13.739, -18.000 18.770 -12.825, -18.000 29.500 -1.575, -18.000 18.223 -11.823, -18.000 28.195 -0.968, -18.000 17.824 -10.754, -18.000 27.143 -0.671, -18.000 17.581 -9.639, -18.000 26.061 -0.520, -18.000 17.500 -8.500, -18.000 24.968 -0.518, -18.000 17.575 -7.409, -18.000 23.884 -0.665, -18.000 17.797 -6.339, -18.000 22.831 -0.958, -18.000 18.164 -5.309, -18.000 21.828 -1.392, -18.000 18.667 -4.339, -18.000 20.894 -1.959, -18.000 19.298 -3.446, -18.000 20.045 -2.648, 14.000 25.827 -0.507, 14.000 17.598 -9.751, 14.000 17.500 -8.500, 14.000 29.500 -1.575, 14.000 29.500 -6.000, 14.000 24.656 -0.545, 14.000 17.586 -7.332, 14.000 18.372 -12.132, 14.000 19.028 -13.202, 14.000 25.833 -16.493, 14.000 23.504 -0.753, 14.000 25.667 -16.498, 14.000 17.841 -6.189, 14.000 24.249 -16.402, 14.000 22.394 -1.127, 14.000 23.028 -16.108, 14.000 21.868 -15.628, 14.000 18.260 -5.096, 14.000 28.122 -0.942, 14.000 17.892 -10.972, 14.000 20.798 -14.972, 14.000 21.351 -1.660, 14.000 18.835 -4.075, 14.000 20.397 -2.339, 14.000 19.552 -3.150, 14.000 19.843 -14.157, 14.000 26.990 -0.640, 14.000 29.749 -8.945, 15.000 28.161 1.492, 15.000 29.456 2.510, 15.000 29.485 1.699, 15.000 29.295 0.909, 15.000 28.900 0.200, 15.000 28.329 -0.378, 15.000 27.624 -0.781, 15.000 26.837 -0.981, 15.000 28.767 3.964, 15.000 29.211 3.284, 15.000 26.220 2.008, 15.000 28.883 3.816, 15.000 28.940 3.742, 15.000 23.500 2.000, 15.000 23.608 2.799, 15.000 23.926 3.541, 15.000 24.430 4.171, 15.000 25.083 4.644, 15.000 25.839 4.926, 15.000 26.643 4.997, 15.000 27.436 4.850, 15.000 28.162 4.498, 15.000 25.998 -0.958, 15.000 25.199 -0.703, 15.000 24.501 -0.237, 15.000 23.960 0.404, 15.000 23.617 1.169, 15.000 28.825 3.891, 15.000 29.211 3.284, 15.000 28.767 3.964, 16.000 29.211 3.284, 16.000 28.767 3.964, 15.000 29.456 2.510, 16.000 29.456 2.510, 15.000 29.485 1.699, 16.000 29.485 1.699, 15.000 29.295 0.909, 16.000 29.295 0.909, 15.000 28.900 0.200, 16.000 28.900 0.200, 15.000 28.329 -0.378, 16.000 28.329 -0.378, 15.000 27.624 -0.781, 16.000 27.624 -0.781, 16.000 26.837 -0.981, 15.000 18.255 -16.886, 15.000 18.878 -16.997, 15.000 18.876 -17.000, 15.000 20.801 -12.766, 15.000 21.221 -13.247, 15.000 21.191 -13.295, 15.000 20.550 -11.916, 15.000 20.286 -12.356, 15.000 19.682 -12.095, 15.000 20.139 -10.484, 15.000 19.031 -12.000, 15.000 18.378 -12.079, 15.000 20.000 -9.000, 15.000 17.768 -12.325, 15.000 20.099 -7.744, 15.000 17.243 -12.721, 15.000 26.787 -1.092, 15.000 25.561 -1.381, 15.000 24.395 -1.858, 15.000 23.319 -2.512, 15.000 22.359 -3.328, 15.000 5.406 -16.422, 15.000 17.189 -16.224, 15.000 17.680 -16.623, 15.000 5.724 -16.013, 15.000 16.815 -15.714, 15.000 4.993 -16.736, 15.000 5.927 -15.536, 15.000 16.580 -15.127, 15.000 4.514 -16.933, 15.000 27.114 5.920, 15.000 6.000 -15.023, 15.000 16.500 -14.500, 15.000 4.000 -17.000, 15.000 5.939 -14.509, 15.000 16.587 -13.848, 15.000 5.747 -14.027, 15.000 16.840 -13.241, 15.000 5.439 -13.611, 15.000 25.317 7.745, 15.000 2.086 -14.419, 15.000 2.001 -14.930, 15.000 2.000 -14.929, 15.000 23.389 9.430, 15.000 21.339 10.964, 15.000 5.444 13.616, 15.000 12.167 15.444, 15.000 14.581 14.587, 15.000 5.743 14.018, 15.000 9.697 16.117, 15.000 5.932 14.483, 15.000 6.000 14.980, 15.000 7.182 16.603, 15.000 5.942 15.478, 15.000 3.129 13.200, 15.000 3.604 13.040, 15.000 3.531 -13.056, 15.000 3.047 -13.241, 15.000 5.762 15.946, 15.000 2.708 13.473, 15.000 2.628 -13.545, 15.000 5.471 16.355, 15.000 2.369 13.842, 15.000 2.300 -13.947, 15.000 5.088 16.678, 15.000 2.132 14.284, 15.000 4.638 16.897, 15.000 4.637 16.896, 15.000 2.000 14.761, 15.000 2.013 14.771, 15.000 15.384 -0.051, 15.000 20.394 -6.520, 15.000 20.878 -5.356, 15.000 21.538 -4.284, 15.000 16.922 13.550, 15.000 19.179 12.341, 15.000 5.054 13.300, 15.000 4.598 13.091, 15.000 4.104 13.003, 15.000 4.047 -13.001, 15.000 4.559 -13.080, 15.000 5.033 -13.288, 16.000 23.608 2.799, 16.000 23.500 2.000, 16.000 26.492 1.998, 16.000 23.926 3.541, 16.000 24.430 4.171, 16.000 25.083 4.644, 16.000 25.839 4.926, 16.000 26.643 4.997, 16.000 27.436 4.850, 16.000 28.162 4.498, 16.000 25.724 -0.898, 16.000 26.500 -1.000, 16.000 25.000 -0.598, 16.000 24.379 -0.121, 16.000 23.902 0.500, 16.000 23.602 1.224, 16.000 26.725 -0.992, 16.000 26.613 -0.998, 15.000 26.787 -1.092, 16.000 26.787 -1.092, 15.000 25.317 7.745, 16.000 25.317 7.745, 16.000 27.114 5.920, 15.000 27.114 5.920, 16.000 4.638 16.897, 16.000 7.182 16.603, 15.000 7.182 16.603, 15.000 9.697 16.117, 16.000 9.697 16.117, 15.000 12.167 15.444, 16.000 12.167 15.444, 15.000 14.581 14.587, 16.000 14.581 14.587, 16.000 16.922 13.550, 15.000 19.179 12.341, 16.000 19.179 12.341, 15.000 21.339 10.964, 16.000 21.339 10.964, 15.000 23.389 9.430, 16.000 23.389 9.430, 15.000 18.876 -17.000, 15.000 18.878 -16.997, 16.000 18.878 -16.997, 16.000 18.876 -17.000, 15.000 18.845 -14.499, 15.000 21.221 -13.247, 16.000 21.221 -13.247, 16.000 21.191 -13.295, 16.000 20.550 -11.916, 15.000 20.550 -11.916, 16.000 20.139 -10.484, 15.000 20.139 -10.484, 16.000 20.000 -9.000, 15.000 20.000 -9.000, 15.000 20.099 -7.744, 16.000 20.099 -7.744, 15.000 20.394 -6.520, 16.000 20.394 -6.520, 15.000 20.878 -5.356, 16.000 20.878 -5.356, 15.000 21.538 -4.284, 16.000 21.538 -4.284, 15.000 22.359 -3.328, 16.000 22.359 -3.328, 16.000 23.319 -2.512, 15.000 24.395 -1.858, 16.000 24.395 -1.858, 15.000 25.561 -1.381, 16.000 25.561 -1.381, 15.000 4.637 16.896, 16.000 4.637 16.896, 15.000 4.006 14.949, 15.000 2.013 14.771, 16.000 2.013 14.771, 16.000 2.000 14.761, 15.000 2.000 -14.929, 16.000 2.000 -14.929, 16.000 2.001 -14.930, 15.000 4.001 -15.000, 15.000 4.000 -17.000, 16.000 4.000 -17.000, 16.000 18.255 -16.886, 16.000 20.801 -12.766, 16.000 20.286 -12.356, 16.000 19.682 -12.095, 16.000 19.031 -12.000, 16.000 18.378 -12.079, 16.000 17.768 -12.325, 16.000 17.243 -12.721, 16.000 17.680 -16.623, 16.000 17.189 -16.224, 16.000 5.406 -16.422, 16.000 5.724 -16.013, 16.000 16.815 -15.714, 16.000 4.993 -16.736, 16.000 16.580 -15.127, 16.000 5.927 -15.536, 16.000 4.514 -16.933, 16.000 16.500 -14.500, 16.000 6.000 -15.023, 16.000 5.939 -14.509, 16.000 16.587 -13.848, 16.000 5.747 -14.027, 16.000 16.840 -13.241, 16.000 5.439 -13.611, 16.000 2.086 -14.419, 16.000 5.390 13.562, 16.000 5.709 13.961, 16.000 5.916 14.428, 16.000 5.999 14.932, 16.000 5.951 15.441, 16.000 3.047 -13.241, 16.000 3.531 -13.056, 16.000 3.426 13.084, 16.000 4.000 13.000, 16.000 5.775 15.921, 16.000 2.628 -13.545, 16.000 2.900 13.330, 16.000 5.484 16.340, 16.000 2.300 -13.947, 16.000 2.466 13.716, 16.000 5.096 16.673, 16.000 2.162 14.211, 16.000 15.384 -0.051, 16.000 4.047 -13.001, 16.000 4.559 -13.080, 16.000 5.033 -13.288, 16.000 4.980 13.257, 16.000 4.507 13.065, 15.000 21.430 -13.912, 16.000 21.426 -13.897, 16.000 21.500 -14.539, 15.000 21.499 -14.570, 16.000 21.406 -15.178, 15.000 21.393 -15.222, 16.000 21.152 -15.772, 15.000 21.120 -15.825, 16.000 20.754 -16.282, 15.000 20.699 -16.334, 16.000 20.239 -16.672, 15.000 20.158 -16.716, 16.000 19.641 -16.916, 15.000 19.537 -16.942, 16.000 19.000 -17.000, 16.000 18.959 -17.000, 16.000 18.918 -16.999, 15.000 21.120 -15.825, 15.000 20.699 -16.334, 15.000 20.158 -16.716, 15.000 21.393 -15.222, 15.000 2.000 15.000, 16.000 2.000 15.000, 15.000 2.073 15.535, 16.000 2.073 15.535, 15.000 2.286 16.031, 16.000 2.286 16.031, 15.000 2.624 16.451, 16.000 2.624 16.451, 15.000 3.062 16.766, 16.000 3.062 16.766, 15.000 3.569 16.953, 16.000 3.569 16.953, 15.000 4.107 16.997, 16.000 4.107 16.997, 15.000 3.318 15.884, 15.000 2.006 14.847, 15.000 2.001 14.924, 15.000 2.000 15.000, 15.000 2.073 15.535, 15.000 2.286 16.031, 15.000 2.624 16.451, 15.000 3.062 16.766, 15.000 3.569 16.953, 15.000 4.107 16.997, 15.000 2.006 14.847, 16.000 2.006 14.847, 15.000 2.001 14.924, 16.000 2.001 14.924, 15.000 2.000 -15.000, 16.000 2.000 -15.000, 15.000 2.000 -14.977, 16.000 2.000 -14.977, 15.000 2.001 -14.953, 16.000 2.001 -14.953, 15.000 3.482 -16.932, 15.000 3.000 -16.732, 15.000 2.586 -16.414, 15.000 2.268 -16.000, 15.000 2.068 -15.518, 15.000 2.000 -15.000, 15.000 2.000 -14.977, 15.000 2.001 -14.953, 15.000 3.482 -16.932, 16.000 3.482 -16.932, 15.000 3.000 -16.732, 16.000 3.000 -16.732, 15.000 2.586 -16.414, 16.000 2.586 -16.414, 15.000 2.268 -16.000, 16.000 2.268 -16.000, 15.000 2.068 -15.518, 16.000 2.068 -15.518, 16.000 4.000 -15.000, 16.000 19.000 -14.500, 16.000 3.999 14.999, -20.000 28.161 1.492, -20.000 29.456 2.510, -20.000 29.485 1.699, -20.000 29.295 0.909, -20.000 28.900 0.200, -20.000 28.329 -0.378, -20.000 27.624 -0.781, -20.000 26.837 -0.981, -20.000 28.767 3.964, -20.000 29.211 3.284, -20.000 26.220 2.008, -20.000 28.883 3.816, -20.000 28.940 3.742, -20.000 23.500 2.000, -20.000 23.608 2.799, -20.000 23.926 3.541, -20.000 24.430 4.171, -20.000 25.083 4.644, -20.000 25.839 4.926, -20.000 26.643 4.997, -20.000 27.436 4.850, -20.000 28.162 4.498, -20.000 25.998 -0.958, -20.000 25.199 -0.703, -20.000 24.501 -0.237, -20.000 23.960 0.404, -20.000 23.617 1.169, -20.000 28.825 3.891, -20.000 28.767 3.964, -19.000 29.211 3.284, -19.000 28.767 3.964, -20.000 29.456 2.510, -19.000 29.456 2.510, -20.000 29.485 1.699, -19.000 29.485 1.699, -20.000 29.295 0.909, -19.000 29.295 0.909, -20.000 28.900 0.200, -19.000 28.900 0.200, -20.000 28.329 -0.378, -19.000 28.329 -0.378, -20.000 27.624 -0.781, -19.000 27.624 -0.781, -19.000 26.837 -0.981, -20.000 18.255 -16.886, -20.000 18.878 -16.997, -20.000 18.876 -17.000, -20.000 20.801 -12.766, -20.000 21.221 -13.247, -20.000 21.191 -13.295, -20.000 20.550 -11.916, -20.000 20.286 -12.356, -20.000 19.682 -12.095, -20.000 20.139 -10.484, -20.000 19.031 -12.000, -20.000 18.378 -12.079, -20.000 20.000 -9.000, -20.000 17.768 -12.325, -20.000 20.099 -7.744, -20.000 17.243 -12.721, -20.000 26.787 -1.092, -20.000 25.561 -1.381, -20.000 24.395 -1.858, -20.000 23.319 -2.512, -20.000 22.359 -3.328, -20.000 5.406 -16.422, -20.000 17.189 -16.224, -20.000 17.680 -16.623, -20.000 5.724 -16.013, -20.000 16.815 -15.714, -20.000 4.993 -16.736, -20.000 5.927 -15.536, -20.000 16.580 -15.127, -20.000 4.514 -16.933, -20.000 27.114 5.920, -20.000 6.000 -15.023, -20.000 16.500 -14.500, -20.000 4.000 -17.000, -20.000 5.939 -14.509, -20.000 16.587 -13.848, -20.000 5.747 -14.027, -20.000 16.840 -13.241, -20.000 5.439 -13.611, -20.000 25.317 7.745, -20.000 2.086 -14.419, -20.000 2.001 -14.930, -20.000 2.000 -14.929, -20.000 23.389 9.430, -20.000 21.339 10.964, -20.000 5.444 13.616, -20.000 12.167 15.444, -20.000 14.581 14.587, -20.000 5.743 14.018, -20.000 9.697 16.117, -20.000 5.932 14.483, -20.000 6.000 14.980, -20.000 7.182 16.603, -20.000 5.942 15.478, -20.000 3.129 13.200, -20.000 3.604 13.040, -20.000 3.531 -13.056, -20.000 3.047 -13.241, -20.000 5.762 15.946, -20.000 2.708 13.473, -20.000 2.628 -13.545, -20.000 5.471 16.355, -20.000 2.369 13.842, -20.000 2.300 -13.947, -20.000 5.088 16.678, -20.000 2.132 14.284, -20.000 4.638 16.897, -20.000 4.637 16.896, -20.000 2.000 14.761, -20.000 2.013 14.771, -20.000 15.384 -0.051, -20.000 20.394 -6.520, -20.000 20.878 -5.356, -20.000 21.538 -4.284, -20.000 16.922 13.550, -20.000 19.179 12.341, -20.000 5.054 13.300, -20.000 4.598 13.091, -20.000 4.104 13.003, -20.000 4.047 -13.001, -20.000 4.559 -13.080, -20.000 5.033 -13.288, -19.000 23.608 2.799, -19.000 23.500 2.000, -19.000 26.492 1.998, -19.000 23.926 3.541, -19.000 24.430 4.171, -19.000 25.083 4.644, -19.000 25.839 4.926, -19.000 26.643 4.997, -19.000 27.436 4.850, -19.000 28.162 4.498, -19.000 25.724 -0.898, -19.000 26.500 -1.000, -19.000 25.000 -0.598, -19.000 24.379 -0.121, -19.000 23.902 0.500, -19.000 23.602 1.224, -19.000 26.725 -0.992, -19.000 26.613 -0.998, -20.000 26.787 -1.092, -19.000 26.787 -1.092, -20.000 25.317 7.745, -19.000 25.317 7.745, -19.000 27.114 5.920, -20.000 27.114 5.920, -20.000 4.638 16.897, -19.000 4.638 16.897, -19.000 7.182 16.603, -20.000 7.182 16.603, -20.000 9.697 16.117, -19.000 9.697 16.117, -20.000 12.167 15.444, -19.000 12.167 15.444, -20.000 14.581 14.587, -19.000 14.581 14.587, -19.000 16.922 13.550, -20.000 19.179 12.341, -19.000 19.179 12.341, -20.000 21.339 10.964, -19.000 21.339 10.964, -19.000 23.389 9.430, -20.000 18.876 -17.000, -20.000 18.878 -16.997, -19.000 18.878 -16.997, -19.000 18.876 -17.000, -20.000 18.845 -14.499, -20.000 21.221 -13.247, -19.000 21.221 -13.247, -19.000 21.191 -13.295, -19.000 20.550 -11.916, -20.000 20.550 -11.916, -19.000 20.139 -10.484, -20.000 20.139 -10.484, -19.000 20.000 -9.000, -20.000 20.000 -9.000, -19.000 20.099 -7.744, -20.000 20.394 -6.520, -19.000 20.394 -6.520, -20.000 20.878 -5.356, -19.000 20.878 -5.356, -20.000 21.538 -4.284, -19.000 21.538 -4.284, -20.000 22.359 -3.328, -19.000 22.359 -3.328, -19.000 23.319 -2.512, -20.000 24.395 -1.858, -19.000 24.395 -1.858, -20.000 25.561 -1.381, -19.000 25.561 -1.381, -20.000 4.637 16.896, -19.000 4.637 16.896, -20.000 4.006 14.949, -20.000 2.013 14.771, -20.000 2.000 14.761, -19.000 2.013 14.771, -19.000 2.000 14.761, -20.000 2.000 -14.929, -19.000 2.000 -14.929, -20.000 2.001 -14.930, -19.000 2.001 -14.930, -20.000 4.001 -15.000, -20.000 4.000 -17.000, -19.000 4.000 -17.000, -19.000 18.255 -16.886, -19.000 20.801 -12.766, -19.000 20.286 -12.356, -19.000 19.682 -12.095, -19.000 19.031 -12.000, -19.000 18.378 -12.079, -19.000 17.768 -12.325, -19.000 17.243 -12.721, -19.000 17.680 -16.623, -19.000 17.189 -16.224, -19.000 5.406 -16.422, -19.000 5.724 -16.013, -19.000 16.815 -15.714, -19.000 4.993 -16.736, -19.000 16.580 -15.127, -19.000 5.927 -15.536, -19.000 4.514 -16.933, -19.000 16.500 -14.500, -19.000 6.000 -15.023, -19.000 5.939 -14.509, -19.000 16.587 -13.848, -19.000 5.747 -14.027, -19.000 16.840 -13.241, -19.000 5.439 -13.611, -19.000 2.086 -14.419, -19.000 5.390 13.562, -19.000 5.709 13.961, -19.000 5.916 14.428, -19.000 5.999 14.932, -19.000 5.951 15.441, -19.000 3.047 -13.241, -19.000 3.531 -13.056, -19.000 3.426 13.084, -19.000 4.000 13.000, -19.000 5.775 15.921, -19.000 2.628 -13.545, -19.000 2.900 13.330, -19.000 5.484 16.340, -19.000 2.300 -13.947, -19.000 2.466 13.716, -19.000 5.096 16.673, -19.000 2.162 14.211, -19.000 15.384 -0.051, -19.000 4.047 -13.001, -19.000 4.559 -13.080, -19.000 5.033 -13.288, -19.000 4.980 13.257, -19.000 4.507 13.065, -20.000 21.430 -13.912, -19.000 21.426 -13.897, -19.000 21.500 -14.539, -20.000 21.499 -14.570, -19.000 21.406 -15.178, -20.000 21.393 -15.222, -19.000 21.152 -15.772, -20.000 21.120 -15.825, -19.000 20.754 -16.282, -20.000 20.699 -16.334, -19.000 20.239 -16.672, -20.000 20.158 -16.716, -19.000 19.641 -16.916, -20.000 19.537 -16.942, -19.000 19.000 -17.000, -19.000 18.959 -17.000, -19.000 18.918 -16.999, -20.000 21.120 -15.825, -20.000 20.699 -16.334, -20.000 20.158 -16.716, -20.000 21.393 -15.222, -20.000 2.000 15.000, -19.000 2.000 15.000, -20.000 2.073 15.535, -19.000 2.073 15.535, -20.000 2.286 16.031, -19.000 2.286 16.031, -20.000 2.624 16.451, -19.000 2.624 16.451, -20.000 3.062 16.766, -19.000 3.062 16.766, -20.000 3.569 16.953, -19.000 3.569 16.953, -20.000 4.107 16.997, -19.000 4.107 16.997, -20.000 3.318 15.884, -20.000 2.006 14.847, -20.000 2.001 14.924, -20.000 2.000 15.000, -20.000 2.073 15.535, -20.000 2.286 16.031, -20.000 2.624 16.451, -20.000 3.062 16.766, -20.000 3.569 16.953, -20.000 4.107 16.997, -20.000 2.006 14.847, -19.000 2.006 14.847, -20.000 2.001 14.924, -19.000 2.001 14.924, -20.000 2.000 -15.000, -19.000 2.000 -15.000, -20.000 2.000 -14.977, -19.000 2.000 -14.977, -20.000 2.001 -14.953, -19.000 2.001 -14.953, -20.000 3.482 -16.932, -20.000 3.000 -16.732, -20.000 2.586 -16.414, -20.000 2.268 -16.000, -20.000 2.068 -15.518, -20.000 2.000 -15.000, -20.000 2.000 -14.977, -20.000 2.001 -14.953, -20.000 3.482 -16.932, -19.000 3.482 -16.932, -20.000 3.000 -16.732, -19.000 3.000 -16.732, -20.000 2.586 -16.414, -19.000 2.586 -16.414, -20.000 2.268 -16.000, -19.000 2.268 -16.000, -20.000 2.068 -15.518, -19.000 2.068 -15.518, -19.000 4.000 -15.000, -19.000 19.000 -14.500, -19.000 3.999 14.999 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 2, 1, 3, -1, 4, 2, 5, -1, 2, 3, 5, -1, 6, 4, 7, -1, 4, 5, 7, -1, 8, 6, 9, -1, 6, 7, 9, -1, 10, 8, 11, -1, 8, 9, 11, -1, 12, 10, 13, -1, 10, 11, 13, -1, 14, 12, 15, -1, 12, 13, 15, -1, 16, 14, 17, -1, 14, 15, 17, -1, 18, 16, 19, -1, 16, 17, 19, -1, 18, 19, 20, -1, 21, 18, 20, -1, 21, 20, 22, -1, 23, 21, 22, -1, 23, 22, 24, -1, 25, 23, 24, -1, 25, 24, 26, -1, 27, 25, 26, -1, 27, 26, 28, -1, 29, 27, 28, -1, 29, 28, 30, -1, 30, 28, 31, -1, 30, 31, 32, -1, 32, 31, 33, -1, 34, 32, 35, -1, 32, 33, 35, -1, 36, 34, 37, -1, 34, 35, 37, -1, 38, 36, 39, -1, 36, 37, 39, -1, 40, 38, 41, -1, 38, 39, 41, -1, 42, 40, 43, -1, 40, 41, 43, -1, 44, 42, 45, -1, 42, 43, 45, -1, 46, 44, 47, -1, 44, 45, 47, -1, 48, 46, 49, -1, 46, 47, 49, -1, 50, 48, 51, -1, 48, 49, 51, -1, 52, 50, 53, -1, 50, 51, 53, -1, 54, 52, 55, -1, 52, 53, 55, -1, 56, 54, 57, -1, 54, 55, 57, -1, 58, 56, 59, -1, 56, 57, 59, -1, 0, 58, 1, -1, 58, 59, 1, -1, 60, 30, 61, -1, 61, 38, 40, -1, 60, 62, 23, -1, 6, 63, 4, -1, 60, 23, 25, -1, 23, 62, 21, -1, 61, 40, 42, -1, 60, 25, 27, -1, 4, 63, 2, -1, 61, 42, 44, -1, 21, 62, 18, -1, 60, 27, 29, -1, 61, 44, 46, -1, 18, 62, 16, -1, 60, 29, 30, -1, 16, 62, 14, -1, 14, 62, 12, -1, 12, 62, 63, -1, 0, 2, 64, -1, 46, 48, 64, -1, 48, 50, 64, -1, 12, 63, 10, -1, 50, 52, 64, -1, 52, 54, 64, -1, 54, 56, 64, -1, 56, 58, 64, -1, 58, 0, 64, -1, 61, 46, 64, -1, 10, 63, 8, -1, 2, 63, 64, -1, 8, 63, 6, -1, 30, 32, 61, -1, 32, 34, 61, -1, 34, 36, 61, -1, 36, 38, 61, -1, 65, 31, 66, -1, 41, 39, 65, -1, 22, 67, 66, -1, 5, 68, 7, -1, 24, 22, 66, -1, 20, 67, 22, -1, 43, 41, 65, -1, 26, 24, 66, -1, 3, 68, 5, -1, 45, 43, 65, -1, 19, 67, 20, -1, 28, 26, 66, -1, 47, 45, 65, -1, 17, 67, 19, -1, 31, 28, 66, -1, 15, 67, 17, -1, 13, 67, 15, -1, 68, 67, 13, -1, 69, 3, 1, -1, 69, 49, 47, -1, 69, 51, 49, -1, 11, 68, 13, -1, 69, 53, 51, -1, 69, 55, 53, -1, 69, 57, 55, -1, 69, 59, 57, -1, 69, 1, 59, -1, 69, 47, 65, -1, 9, 68, 11, -1, 69, 68, 3, -1, 7, 68, 9, -1, 65, 33, 31, -1, 65, 35, 33, -1, 65, 37, 35, -1, 65, 39, 37, -1, 60, 70, 71, -1, 60, 71, 62, -1, 71, 72, 62, -1, 72, 63, 62, -1, 61, 66, 60, -1, 65, 66, 61, -1, 64, 63, 69, -1, 63, 68, 69, -1, 61, 64, 73, -1, 64, 74, 73, -1, 75, 76, 77, -1, 76, 78, 77, -1, 78, 79, 77, -1, 79, 80, 77, -1, 80, 81, 77, -1, 81, 82, 77, -1, 82, 83, 77, -1, 84, 85, 86, -1, 85, 87, 86, -1, 87, 88, 86, -1, 88, 89, 86, -1, 89, 90, 86, -1, 90, 91, 86, -1, 91, 92, 86, -1, 92, 93, 86, -1, 93, 94, 86, -1, 83, 95, 68, -1, 95, 96, 68, -1, 77, 83, 68, -1, 84, 86, 67, -1, 75, 77, 97, -1, 86, 94, 98, -1, 67, 68, 99, -1, 96, 100, 99, -1, 100, 101, 99, -1, 101, 102, 99, -1, 102, 103, 99, -1, 103, 104, 99, -1, 104, 84, 99, -1, 84, 67, 99, -1, 68, 96, 99, -1, 105, 106, 107, -1, 105, 108, 106, -1, 105, 109, 108, -1, 105, 110, 109, -1, 105, 111, 110, -1, 105, 112, 111, -1, 69, 113, 112, -1, 69, 114, 113, -1, 69, 115, 114, -1, 69, 112, 105, -1, 116, 117, 118, -1, 116, 119, 117, -1, 116, 120, 119, -1, 116, 121, 120, -1, 122, 105, 107, -1, 123, 121, 116, -1, 123, 116, 124, -1, 124, 116, 125, -1, 126, 65, 116, -1, 126, 69, 65, -1, 126, 127, 115, -1, 126, 128, 127, -1, 126, 129, 128, -1, 126, 130, 129, -1, 126, 131, 130, -1, 126, 132, 131, -1, 126, 118, 132, -1, 126, 115, 69, -1, 126, 116, 118, -1, 133, 134, 66, -1, 67, 133, 66, -1, 133, 71, 70, -1, 133, 70, 134, -1, 60, 134, 70, -1, 66, 134, 60, -1, 72, 71, 133, -1, 135, 72, 133, -1, 72, 136, 63, -1, 136, 137, 63, -1, 138, 65, 61, -1, 138, 61, 73, -1, 68, 63, 137, -1, 68, 137, 77, -1, 139, 64, 69, -1, 105, 139, 69, -1, 138, 73, 74, -1, 138, 74, 140, -1, 64, 139, 74, -1, 139, 141, 74, -1, 133, 67, 86, -1, 133, 86, 142, -1, 143, 97, 77, -1, 143, 77, 144, -1, 145, 146, 147, -1, 148, 145, 147, -1, 149, 148, 147, -1, 150, 149, 151, -1, 149, 147, 151, -1, 152, 150, 153, -1, 154, 152, 153, -1, 150, 151, 153, -1, 155, 154, 156, -1, 154, 153, 156, -1, 155, 156, 157, -1, 155, 157, 158, -1, 158, 157, 159, -1, 158, 159, 160, -1, 160, 159, 161, -1, 160, 161, 162, -1, 163, 164, 165, -1, 164, 166, 165, -1, 167, 163, 168, -1, 163, 165, 168, -1, 162, 161, 169, -1, 161, 170, 169, -1, 171, 167, 172, -1, 167, 168, 172, -1, 169, 170, 173, -1, 170, 174, 173, -1, 171, 172, 175, -1, 174, 176, 177, -1, 173, 174, 177, -1, 178, 171, 179, -1, 171, 175, 179, -1, 177, 176, 180, -1, 181, 178, 182, -1, 178, 179, 182, -1, 176, 183, 184, -1, 180, 176, 184, -1, 185, 181, 186, -1, 181, 182, 186, -1, 183, 187, 188, -1, 184, 183, 188, -1, 185, 186, 189, -1, 187, 190, 191, -1, 188, 187, 191, -1, 192, 185, 193, -1, 185, 189, 193, -1, 191, 190, 194, -1, 195, 192, 196, -1, 192, 193, 196, -1, 190, 197, 198, -1, 194, 190, 198, -1, 197, 195, 199, -1, 195, 196, 199, -1, 198, 197, 199, -1, 200, 201, 202, -1, 203, 200, 202, -1, 138, 204, 116, -1, 65, 138, 116, -1, 205, 206, 105, -1, 122, 205, 105, -1, 207, 208, 209, -1, 210, 211, 212, -1, 207, 213, 208, -1, 214, 215, 216, -1, 214, 217, 215, -1, 218, 211, 210, -1, 219, 207, 209, -1, 218, 220, 211, -1, 221, 222, 223, -1, 221, 223, 224, -1, 225, 226, 227, -1, 225, 227, 228, -1, 229, 214, 216, -1, 230, 231, 220, -1, 232, 228, 233, -1, 230, 220, 218, -1, 232, 233, 234, -1, 232, 225, 228, -1, 235, 229, 216, -1, 235, 216, 236, -1, 237, 234, 238, -1, 237, 238, 239, -1, 237, 232, 234, -1, 240, 209, 241, -1, 240, 219, 209, -1, 242, 231, 230, -1, 243, 244, 222, -1, 245, 239, 246, -1, 247, 236, 248, -1, 247, 235, 236, -1, 243, 222, 221, -1, 245, 237, 239, -1, 249, 250, 231, -1, 249, 231, 242, -1, 251, 247, 248, -1, 252, 245, 246, -1, 253, 248, 250, -1, 253, 250, 249, -1, 253, 251, 248, -1, 254, 252, 246, -1, 208, 213, 255, -1, 256, 240, 241, -1, 212, 244, 243, -1, 212, 211, 244, -1, 223, 252, 254, -1, 224, 223, 254, -1, 217, 241, 215, -1, 217, 256, 241, -1, 257, 258, 259, -1, 257, 259, 260, -1, 136, 72, 135, -1, 144, 136, 135, -1, 144, 261, 262, -1, 144, 263, 261, -1, 144, 264, 263, -1, 144, 265, 264, -1, 144, 266, 265, -1, 144, 267, 266, -1, 144, 268, 267, -1, 142, 269, 270, -1, 142, 271, 269, -1, 142, 272, 271, -1, 142, 273, 272, -1, 142, 274, 273, -1, 142, 275, 274, -1, 142, 276, 275, -1, 142, 277, 276, -1, 142, 278, 277, -1, 135, 279, 268, -1, 135, 280, 279, -1, 135, 268, 144, -1, 133, 142, 270, -1, 143, 144, 262, -1, 281, 278, 142, -1, 282, 283, 280, -1, 282, 284, 283, -1, 282, 285, 284, -1, 282, 286, 285, -1, 282, 287, 286, -1, 282, 270, 287, -1, 282, 135, 133, -1, 282, 133, 270, -1, 282, 280, 135, -1, 137, 136, 77, -1, 136, 144, 77, -1, 141, 139, 206, -1, 139, 105, 206, -1, 140, 74, 141, -1, 140, 141, 206, -1, 288, 289, 206, -1, 289, 290, 206, -1, 290, 291, 206, -1, 291, 292, 206, -1, 292, 293, 206, -1, 293, 294, 206, -1, 294, 295, 140, -1, 295, 296, 140, -1, 296, 297, 140, -1, 206, 294, 140, -1, 298, 299, 204, -1, 299, 300, 204, -1, 300, 301, 204, -1, 301, 302, 204, -1, 288, 206, 205, -1, 204, 302, 303, -1, 304, 204, 303, -1, 305, 204, 304, -1, 297, 306, 307, -1, 306, 308, 307, -1, 308, 309, 307, -1, 309, 310, 307, -1, 310, 311, 307, -1, 311, 312, 307, -1, 312, 298, 307, -1, 204, 138, 307, -1, 138, 140, 307, -1, 140, 297, 307, -1, 298, 204, 307, -1, 313, 149, 314, -1, 315, 313, 314, -1, 313, 146, 145, -1, 313, 145, 148, -1, 313, 148, 149, -1, 166, 316, 317, -1, 318, 319, 320, -1, 319, 321, 320, -1, 166, 317, 168, -1, 165, 168, 166, -1, 322, 323, 324, -1, 325, 322, 324, -1, 326, 325, 324, -1, 327, 318, 328, -1, 318, 320, 328, -1, 323, 329, 330, -1, 324, 323, 330, -1, 146, 327, 147, -1, 327, 328, 147, -1, 329, 331, 332, -1, 330, 329, 332, -1, 333, 334, 335, -1, 331, 333, 335, -1, 332, 331, 335, -1, 334, 336, 337, -1, 335, 334, 337, -1, 338, 339, 340, -1, 336, 341, 342, -1, 337, 336, 342, -1, 343, 338, 321, -1, 319, 343, 321, -1, 338, 340, 321, -1, 341, 344, 166, -1, 342, 341, 166, -1, 339, 325, 326, -1, 166, 344, 316, -1, 340, 339, 326, -1, 345, 346, 347, -1, 346, 348, 347, -1, 349, 345, 350, -1, 345, 347, 350, -1, 351, 349, 352, -1, 349, 350, 352, -1, 353, 351, 354, -1, 351, 352, 354, -1, 355, 353, 356, -1, 353, 354, 356, -1, 355, 356, 357, -1, 357, 356, 358, -1, 357, 358, 359, -1, 360, 357, 359, -1, 360, 359, 361, -1, 361, 359, 362, -1, 361, 362, 363, -1, 364, 361, 363, -1, 364, 363, 365, -1, 365, 363, 366, -1, 365, 366, 367, -1, 367, 366, 368, -1, 367, 368, 369, -1, 370, 367, 369, -1, 370, 369, 371, -1, 371, 369, 372, -1, 371, 372, 373, -1, 374, 371, 373, -1, 374, 373, 375, -1, 375, 373, 376, -1, 375, 376, 377, -1, 377, 376, 378, -1, 377, 378, 379, -1, 380, 377, 379, -1, 380, 379, 381, -1, 381, 379, 382, -1, 383, 381, 384, -1, 381, 382, 384, -1, 383, 384, 385, -1, 385, 384, 386, -1, 385, 386, 387, -1, 387, 386, 388, -1, 389, 387, 390, -1, 387, 388, 390, -1, 389, 390, 391, -1, 391, 390, 392, -1, 391, 392, 393, -1, 393, 392, 394, -1, 395, 393, 396, -1, 393, 394, 396, -1, 395, 396, 397, -1, 397, 396, 398, -1, 399, 397, 400, -1, 397, 398, 400, -1, 401, 399, 402, -1, 399, 400, 402, -1, 403, 401, 404, -1, 401, 402, 404, -1, 405, 403, 406, -1, 403, 404, 406, -1, 405, 406, 407, -1, 407, 406, 408, -1, 409, 407, 410, -1, 407, 408, 410, -1, 409, 410, 411, -1, 412, 409, 411, -1, 412, 411, 413, -1, 414, 412, 413, -1, 414, 413, 346, -1, 346, 413, 348, -1, 415, 416, 417, -1, 418, 416, 415, -1, 419, 202, 201, -1, 420, 418, 415, -1, 421, 202, 419, -1, 422, 418, 420, -1, 423, 422, 420, -1, 424, 421, 419, -1, 425, 422, 423, -1, 426, 421, 424, -1, 427, 423, 428, -1, 429, 424, 430, -1, 427, 425, 423, -1, 429, 426, 424, -1, 431, 427, 428, -1, 432, 429, 430, -1, 431, 428, 433, -1, 434, 429, 432, -1, 435, 431, 433, -1, 436, 434, 432, -1, 435, 433, 437, -1, 438, 434, 436, -1, 439, 435, 437, -1, 440, 435, 439, -1, 441, 438, 436, -1, 442, 440, 439, -1, 443, 438, 441, -1, 444, 440, 442, -1, 445, 441, 446, -1, 445, 443, 441, -1, 447, 444, 442, -1, 447, 442, 448, -1, 449, 445, 446, -1, 450, 445, 449, -1, 451, 449, 452, -1, 451, 450, 449, -1, 416, 452, 417, -1, 416, 451, 452, -1, 453, 454, 455, -1, 456, 453, 455, -1, 227, 226, 457, -1, 228, 227, 457, -1, 233, 228, 457, -1, 458, 459, 460, -1, 458, 460, 461, -1, 458, 461, 462, -1, 462, 461, 463, -1, 458, 464, 465, -1, 462, 463, 466, -1, 213, 219, 207, -1, 467, 468, 469, -1, 467, 470, 468, -1, 225, 469, 226, -1, 225, 467, 469, -1, 471, 472, 465, -1, 471, 465, 464, -1, 213, 471, 473, -1, 213, 474, 475, -1, 213, 475, 472, -1, 213, 472, 471, -1, 476, 477, 478, -1, 479, 474, 213, -1, 480, 479, 213, -1, 466, 463, 481, -1, 466, 481, 477, -1, 466, 477, 476, -1, 470, 478, 482, -1, 470, 482, 468, -1, 219, 480, 213, -1, 473, 255, 213, -1, 470, 476, 478, -1, 458, 483, 464, -1, 458, 462, 483, -1, 458, 465, 459, -1, 484, 485, 486, -1, 485, 487, 486, -1, 488, 484, 489, -1, 484, 486, 489, -1, 490, 488, 491, -1, 488, 489, 491, -1, 492, 490, 493, -1, 490, 491, 493, -1, 494, 492, 495, -1, 492, 493, 495, -1, 496, 494, 497, -1, 494, 495, 497, -1, 496, 497, 498, -1, 498, 497, 499, -1, 500, 498, 501, -1, 498, 499, 501, -1, 500, 501, 502, -1, 502, 501, 503, -1, 502, 503, 504, -1, 505, 502, 504, -1, 505, 504, 506, -1, 506, 504, 507, -1, 506, 507, 508, -1, 509, 506, 508, -1, 509, 508, 510, -1, 511, 509, 510, -1, 511, 510, 512, -1, 513, 511, 512, -1, 513, 512, 514, -1, 515, 513, 514, -1, 515, 514, 516, -1, 517, 515, 516, -1, 517, 516, 518, -1, 519, 517, 518, -1, 519, 518, 520, -1, 521, 519, 520, -1, 521, 520, 522, -1, 523, 521, 522, -1, 523, 522, 524, -1, 525, 523, 524, -1, 525, 524, 526, -1, 527, 525, 526, -1, 527, 526, 528, -1, 528, 526, 529, -1, 528, 529, 530, -1, 530, 529, 531, -1, 530, 531, 532, -1, 532, 531, 533, -1, 534, 532, 535, -1, 532, 533, 535, -1, 536, 534, 537, -1, 534, 535, 537, -1, 538, 536, 539, -1, 536, 537, 539, -1, 538, 539, 540, -1, 540, 539, 541, -1, 540, 541, 485, -1, 485, 541, 487, -1, 542, 543, 544, -1, 545, 544, 546, -1, 547, 260, 259, -1, 545, 542, 544, -1, 548, 545, 546, -1, 549, 260, 547, -1, 550, 545, 548, -1, 551, 550, 548, -1, 552, 547, 553, -1, 554, 550, 551, -1, 552, 549, 547, -1, 555, 551, 556, -1, 557, 553, 558, -1, 555, 554, 551, -1, 557, 552, 553, -1, 559, 555, 556, -1, 560, 555, 559, -1, 561, 558, 562, -1, 563, 560, 559, -1, 561, 557, 558, -1, 564, 560, 563, -1, 565, 561, 562, -1, 566, 564, 563, -1, 567, 561, 565, -1, 566, 563, 568, -1, 569, 565, 570, -1, 569, 567, 565, -1, 571, 570, 572, -1, 571, 569, 570, -1, 573, 571, 572, -1, 574, 571, 573, -1, 543, 573, 575, -1, 543, 574, 573, -1, 544, 543, 575, -1, 398, 447, 576, -1, 398, 576, 577, -1, 398, 577, 314, -1, 400, 314, 578, -1, 400, 398, 314, -1, 402, 578, 579, -1, 402, 579, 580, -1, 402, 400, 578, -1, 404, 580, 581, -1, 404, 402, 580, -1, 406, 404, 581, -1, 582, 406, 581, -1, 408, 406, 582, -1, 583, 408, 582, -1, 410, 408, 583, -1, 584, 410, 583, -1, 585, 372, 369, -1, 585, 373, 372, -1, 202, 369, 368, -1, 202, 585, 369, -1, 586, 410, 584, -1, 586, 411, 410, -1, 587, 368, 366, -1, 587, 202, 368, -1, 588, 411, 586, -1, 588, 413, 411, -1, 589, 587, 366, -1, 590, 413, 588, -1, 590, 348, 413, -1, 591, 589, 366, -1, 591, 366, 363, -1, 592, 348, 590, -1, 593, 591, 363, -1, 593, 363, 362, -1, 594, 348, 592, -1, 594, 347, 348, -1, 595, 362, 359, -1, 595, 593, 362, -1, 596, 350, 347, -1, 596, 347, 594, -1, 597, 595, 359, -1, 598, 352, 350, -1, 598, 350, 596, -1, 599, 359, 358, -1, 599, 597, 359, -1, 600, 352, 598, -1, 601, 599, 358, -1, 601, 358, 356, -1, 602, 354, 352, -1, 602, 352, 600, -1, 603, 356, 354, -1, 603, 601, 356, -1, 603, 354, 602, -1, 498, 500, 604, -1, 605, 485, 606, -1, 502, 505, 607, -1, 505, 608, 607, -1, 498, 604, 609, -1, 496, 498, 609, -1, 502, 607, 259, -1, 606, 485, 610, -1, 485, 484, 610, -1, 536, 538, 611, -1, 612, 536, 611, -1, 613, 568, 527, -1, 496, 609, 614, -1, 615, 613, 527, -1, 610, 484, 616, -1, 454, 615, 528, -1, 484, 488, 616, -1, 617, 454, 528, -1, 615, 527, 528, -1, 496, 614, 618, -1, 619, 617, 530, -1, 620, 619, 530, -1, 494, 496, 618, -1, 616, 488, 621, -1, 617, 528, 530, -1, 500, 502, 622, -1, 502, 259, 622, -1, 538, 540, 623, -1, 624, 620, 532, -1, 620, 530, 532, -1, 492, 494, 625, -1, 611, 538, 623, -1, 494, 618, 625, -1, 621, 488, 626, -1, 488, 490, 626, -1, 492, 625, 627, -1, 624, 532, 534, -1, 492, 627, 628, -1, 490, 492, 628, -1, 626, 490, 628, -1, 624, 534, 629, -1, 506, 608, 505, -1, 500, 622, 630, -1, 623, 540, 605, -1, 540, 485, 605, -1, 629, 534, 536, -1, 629, 536, 612, -1, 500, 630, 604, -1, 576, 447, 315, -1, 577, 576, 315, -1, 314, 577, 315, -1, 448, 313, 447, -1, 447, 313, 315, -1, 421, 426, 373, -1, 394, 435, 440, -1, 394, 392, 435, -1, 202, 421, 373, -1, 202, 585, 373, -1, 386, 416, 418, -1, 386, 418, 422, -1, 386, 422, 388, -1, 396, 440, 444, -1, 396, 394, 440, -1, 384, 451, 416, -1, 384, 416, 386, -1, 398, 444, 447, -1, 398, 396, 444, -1, 382, 450, 451, -1, 382, 451, 384, -1, 379, 443, 445, -1, 379, 445, 450, -1, 379, 450, 382, -1, 378, 438, 443, -1, 390, 425, 427, -1, 378, 443, 379, -1, 376, 434, 438, -1, 376, 438, 378, -1, 392, 427, 431, -1, 392, 431, 435, -1, 373, 429, 434, -1, 392, 390, 427, -1, 388, 422, 425, -1, 373, 434, 376, -1, 426, 429, 373, -1, 388, 425, 390, -1, 453, 568, 613, -1, 453, 613, 615, -1, 453, 615, 454, -1, 568, 453, 566, -1, 566, 453, 456, -1, 563, 559, 525, -1, 559, 523, 525, -1, 568, 563, 527, -1, 563, 525, 527, -1, 570, 565, 511, -1, 513, 570, 511, -1, 562, 558, 608, -1, 565, 562, 608, -1, 509, 511, 608, -1, 511, 565, 608, -1, 551, 548, 521, -1, 509, 608, 506, -1, 608, 558, 553, -1, 608, 553, 547, -1, 546, 544, 519, -1, 548, 546, 519, -1, 521, 548, 519, -1, 556, 551, 523, -1, 559, 556, 523, -1, 506, 547, 259, -1, 547, 607, 608, -1, 572, 570, 631, -1, 573, 572, 631, -1, 575, 573, 631, -1, 551, 521, 523, -1, 513, 515, 631, -1, 515, 517, 631, -1, 517, 575, 631, -1, 570, 513, 631, -1, 544, 575, 517, -1, 519, 544, 517, -1, 632, 633, 634, -1, 632, 634, 635, -1, 632, 635, 636, -1, 632, 636, 637, -1, 632, 637, 638, -1, 632, 638, 639, -1, 640, 641, 633, -1, 642, 643, 644, -1, 642, 645, 646, -1, 642, 646, 647, -1, 642, 647, 648, -1, 642, 648, 649, -1, 642, 649, 650, -1, 642, 650, 651, -1, 642, 651, 652, -1, 642, 652, 653, -1, 642, 653, 640, -1, 642, 639, 654, -1, 642, 654, 655, -1, 642, 655, 656, -1, 642, 656, 657, -1, 642, 657, 658, -1, 642, 658, 645, -1, 642, 644, 639, -1, 642, 640, 659, -1, 642, 659, 643, -1, 660, 661, 662, -1, 661, 663, 662, -1, 664, 660, 665, -1, 660, 662, 665, -1, 666, 664, 667, -1, 664, 665, 667, -1, 668, 666, 669, -1, 666, 667, 669, -1, 670, 668, 671, -1, 668, 669, 671, -1, 672, 670, 673, -1, 670, 671, 673, -1, 674, 672, 675, -1, 672, 673, 675, -1, 639, 674, 676, -1, 674, 675, 676, -1, 677, 678, 679, -1, 680, 681, 682, -1, 683, 681, 680, -1, 683, 680, 684, -1, 685, 683, 684, -1, 686, 685, 687, -1, 686, 683, 685, -1, 688, 686, 687, -1, 689, 688, 690, -1, 689, 686, 688, -1, 691, 690, 692, -1, 691, 689, 690, -1, 654, 639, 693, -1, 694, 654, 693, -1, 655, 694, 695, -1, 655, 654, 694, -1, 656, 695, 696, -1, 656, 655, 695, -1, 657, 656, 696, -1, 658, 696, 697, -1, 658, 657, 696, -1, 698, 699, 700, -1, 701, 702, 699, -1, 701, 699, 698, -1, 703, 700, 677, -1, 703, 698, 700, -1, 704, 705, 702, -1, 704, 702, 701, -1, 706, 677, 679, -1, 706, 703, 677, -1, 707, 652, 651, -1, 707, 653, 652, -1, 707, 640, 653, -1, 708, 709, 705, -1, 708, 705, 704, -1, 710, 706, 679, -1, 650, 707, 651, -1, 711, 712, 709, -1, 711, 709, 708, -1, 713, 714, 712, -1, 713, 712, 711, -1, 715, 692, 714, -1, 715, 714, 713, -1, 716, 649, 648, -1, 716, 650, 649, -1, 716, 707, 650, -1, 717, 718, 719, -1, 720, 716, 648, -1, 720, 648, 647, -1, 721, 720, 647, -1, 722, 723, 724, -1, 725, 723, 722, -1, 725, 726, 723, -1, 727, 726, 725, -1, 728, 726, 727, -1, 729, 726, 728, -1, 730, 729, 728, -1, 731, 732, 733, -1, 731, 733, 734, -1, 735, 729, 730, -1, 736, 731, 734, -1, 736, 734, 737, -1, 738, 729, 735, -1, 739, 736, 737, -1, 739, 737, 740, -1, 741, 729, 738, -1, 742, 740, 717, -1, 742, 739, 740, -1, 743, 729, 741, -1, 744, 743, 741, -1, 745, 746, 742, -1, 719, 745, 717, -1, 717, 745, 742, -1, 747, 692, 715, -1, 747, 748, 691, -1, 747, 749, 748, -1, 747, 750, 749, -1, 747, 697, 750, -1, 747, 645, 658, -1, 747, 646, 645, -1, 747, 647, 646, -1, 747, 722, 724, -1, 747, 691, 692, -1, 747, 724, 751, -1, 747, 751, 752, -1, 747, 752, 721, -1, 747, 753, 722, -1, 747, 754, 753, -1, 747, 755, 754, -1, 747, 733, 732, -1, 747, 732, 755, -1, 747, 658, 697, -1, 747, 756, 733, -1, 747, 757, 756, -1, 747, 758, 757, -1, 747, 715, 758, -1, 747, 721, 647, -1, 759, 760, 761, -1, 762, 759, 761, -1, 763, 762, 761, -1, 764, 763, 761, -1, 765, 764, 761, -1, 766, 765, 761, -1, 767, 766, 761, -1, 768, 767, 761, -1, 663, 768, 761, -1, 769, 770, 761, -1, 771, 769, 761, -1, 772, 771, 761, -1, 773, 772, 761, -1, 774, 773, 761, -1, 760, 774, 761, -1, 775, 676, 761, -1, 776, 775, 761, -1, 770, 776, 761, -1, 662, 663, 761, -1, 665, 662, 761, -1, 667, 665, 761, -1, 669, 667, 761, -1, 671, 669, 761, -1, 673, 671, 761, -1, 675, 673, 761, -1, 676, 675, 761, -1, 777, 639, 778, -1, 778, 639, 676, -1, 779, 780, 781, -1, 661, 782, 663, -1, 782, 781, 663, -1, 743, 783, 784, -1, 785, 743, 784, -1, 786, 785, 787, -1, 785, 784, 787, -1, 788, 786, 789, -1, 786, 787, 789, -1, 790, 788, 791, -1, 788, 789, 791, -1, 751, 790, 792, -1, 790, 791, 792, -1, 793, 751, 794, -1, 751, 792, 794, -1, 795, 793, 796, -1, 793, 794, 796, -1, 797, 795, 798, -1, 795, 796, 798, -1, 779, 797, 780, -1, 797, 798, 780, -1, 782, 779, 781, -1, 799, 800, 801, -1, 802, 799, 801, -1, 803, 682, 678, -1, 803, 709, 712, -1, 803, 712, 714, -1, 803, 714, 692, -1, 803, 692, 690, -1, 803, 690, 688, -1, 803, 688, 687, -1, 803, 687, 685, -1, 803, 685, 684, -1, 803, 684, 680, -1, 803, 680, 682, -1, 803, 678, 677, -1, 803, 677, 700, -1, 803, 700, 699, -1, 803, 699, 702, -1, 803, 702, 705, -1, 803, 705, 709, -1, 682, 804, 805, -1, 806, 682, 805, -1, 807, 805, 804, -1, 807, 804, 808, -1, 809, 808, 810, -1, 809, 807, 808, -1, 811, 810, 812, -1, 811, 809, 810, -1, 813, 811, 812, -1, 814, 811, 813, -1, 815, 814, 813, -1, 816, 814, 815, -1, 817, 816, 815, -1, 818, 816, 817, -1, 819, 818, 817, -1, 820, 818, 819, -1, 821, 820, 819, -1, 822, 820, 821, -1, 696, 822, 821, -1, 823, 822, 696, -1, 824, 823, 696, -1, 825, 823, 824, -1, 826, 825, 824, -1, 827, 825, 826, -1, 777, 827, 826, -1, 778, 827, 777, -1, 743, 828, 783, -1, 783, 828, 829, -1, 830, 741, 738, -1, 830, 738, 735, -1, 830, 735, 730, -1, 830, 730, 728, -1, 830, 728, 727, -1, 830, 727, 725, -1, 830, 725, 722, -1, 830, 722, 753, -1, 830, 753, 754, -1, 830, 754, 755, -1, 830, 755, 732, -1, 830, 732, 731, -1, 830, 731, 736, -1, 830, 736, 739, -1, 830, 739, 742, -1, 830, 742, 746, -1, 830, 746, 744, -1, 830, 744, 741, -1, 831, 745, 832, -1, 832, 745, 833, -1, 833, 745, 834, -1, 833, 834, 835, -1, 834, 718, 836, -1, 835, 834, 836, -1, 837, 734, 733, -1, 837, 733, 756, -1, 837, 756, 757, -1, 837, 757, 758, -1, 837, 758, 715, -1, 837, 715, 713, -1, 837, 713, 711, -1, 837, 711, 708, -1, 837, 708, 704, -1, 837, 704, 701, -1, 837, 701, 698, -1, 837, 698, 703, -1, 837, 703, 706, -1, 837, 706, 710, -1, 837, 710, 718, -1, 837, 718, 717, -1, 837, 717, 740, -1, 837, 740, 737, -1, 837, 737, 734, -1, 838, 799, 802, -1, 839, 838, 802, -1, 802, 801, 840, -1, 806, 805, 841, -1, 841, 805, 807, -1, 842, 841, 807, -1, 842, 807, 843, -1, 843, 807, 809, -1, 844, 843, 809, -1, 844, 809, 845, -1, 845, 809, 811, -1, 846, 845, 811, -1, 846, 811, 814, -1, 847, 846, 814, -1, 778, 676, 775, -1, 778, 775, 776, -1, 778, 776, 770, -1, 778, 770, 827, -1, 827, 770, 769, -1, 827, 769, 771, -1, 825, 827, 771, -1, 825, 771, 772, -1, 823, 825, 772, -1, 822, 823, 773, -1, 823, 772, 773, -1, 822, 773, 774, -1, 848, 849, 850, -1, 850, 849, 851, -1, 849, 852, 851, -1, 840, 848, 853, -1, 848, 850, 853, -1, 852, 854, 855, -1, 851, 852, 855, -1, 802, 840, 856, -1, 840, 853, 856, -1, 766, 767, 781, -1, 767, 768, 781, -1, 768, 663, 781, -1, 854, 857, 858, -1, 855, 854, 858, -1, 802, 856, 839, -1, 766, 781, 765, -1, 858, 857, 859, -1, 857, 860, 859, -1, 859, 860, 861, -1, 860, 862, 861, -1, 861, 862, 863, -1, 862, 847, 863, -1, 763, 764, 780, -1, 764, 765, 780, -1, 765, 781, 780, -1, 835, 836, 864, -1, 763, 780, 798, -1, 762, 763, 798, -1, 762, 798, 796, -1, 791, 789, 865, -1, 865, 789, 866, -1, 789, 787, 866, -1, 866, 787, 867, -1, 867, 787, 868, -1, 868, 787, 784, -1, 868, 784, 869, -1, 870, 871, 872, -1, 871, 873, 872, -1, 869, 784, 874, -1, 875, 870, 876, -1, 870, 872, 876, -1, 874, 784, 877, -1, 878, 875, 879, -1, 875, 876, 879, -1, 877, 784, 880, -1, 864, 878, 881, -1, 878, 879, 881, -1, 880, 784, 783, -1, 880, 783, 829, -1, 881, 832, 833, -1, 864, 833, 835, -1, 881, 833, 864, -1, 863, 847, 882, -1, 871, 883, 882, -1, 883, 884, 882, -1, 884, 885, 882, -1, 885, 863, 882, -1, 762, 796, 882, -1, 873, 871, 882, -1, 847, 814, 882, -1, 865, 886, 882, -1, 886, 887, 882, -1, 887, 873, 882, -1, 791, 865, 882, -1, 792, 791, 882, -1, 794, 792, 882, -1, 796, 794, 882, -1, 814, 816, 882, -1, 816, 818, 882, -1, 818, 820, 882, -1, 820, 822, 882, -1, 760, 759, 882, -1, 759, 762, 882, -1, 822, 774, 882, -1, 774, 760, 882, -1, 888, 682, 889, -1, 682, 806, 889, -1, 888, 889, 890, -1, 888, 890, 891, -1, 891, 890, 892, -1, 891, 892, 893, -1, 893, 892, 894, -1, 893, 894, 895, -1, 895, 894, 896, -1, 895, 896, 897, -1, 897, 896, 898, -1, 897, 898, 899, -1, 899, 898, 900, -1, 899, 900, 901, -1, 901, 900, 902, -1, 902, 903, 800, -1, 903, 904, 800, -1, 901, 902, 800, -1, 800, 904, 801, -1, 678, 905, 906, -1, 678, 906, 907, -1, 678, 907, 901, -1, 682, 888, 891, -1, 682, 891, 908, -1, 682, 908, 905, -1, 682, 905, 678, -1, 909, 910, 911, -1, 911, 910, 912, -1, 911, 912, 913, -1, 913, 912, 914, -1, 913, 914, 915, -1, 915, 914, 916, -1, 915, 916, 917, -1, 917, 916, 918, -1, 917, 918, 919, -1, 919, 918, 920, -1, 919, 920, 921, -1, 921, 920, 922, -1, 921, 922, 828, -1, 828, 922, 829, -1, 923, 746, 924, -1, 923, 924, 925, -1, 923, 925, 926, -1, 923, 744, 746, -1, 923, 926, 927, -1, 923, 927, 928, -1, 923, 928, 929, -1, 923, 929, 930, -1, 923, 930, 931, -1, 923, 931, 932, -1, 923, 932, 744, -1, 831, 832, 933, -1, 933, 832, 934, -1, 933, 934, 935, -1, 935, 934, 936, -1, 935, 936, 909, -1, 909, 936, 910, -1, 937, 938, 939, -1, 939, 938, 940, -1, 939, 940, 941, -1, 941, 940, 942, -1, 941, 942, 718, -1, 718, 942, 836, -1, 718, 710, 943, -1, 718, 943, 944, -1, 718, 944, 945, -1, 718, 945, 946, -1, 718, 946, 947, -1, 718, 947, 948, -1, 718, 948, 949, -1, 718, 949, 950, -1, 951, 838, 952, -1, 838, 839, 952, -1, 953, 951, 954, -1, 951, 952, 954, -1, 955, 953, 956, -1, 953, 954, 956, -1, 957, 955, 958, -1, 955, 956, 958, -1, 959, 957, 960, -1, 957, 958, 960, -1, 959, 960, 937, -1, 937, 960, 938, -1, 940, 938, 961, -1, 942, 940, 961, -1, 836, 942, 961, -1, 952, 839, 961, -1, 954, 952, 961, -1, 956, 954, 961, -1, 958, 956, 961, -1, 960, 958, 961, -1, 938, 960, 961, -1, 864, 836, 961, -1, 878, 864, 961, -1, 875, 878, 961, -1, 870, 875, 961, -1, 871, 870, 961, -1, 883, 871, 961, -1, 884, 883, 961, -1, 885, 884, 961, -1, 863, 885, 961, -1, 861, 863, 961, -1, 859, 861, 961, -1, 858, 859, 961, -1, 855, 858, 961, -1, 851, 855, 961, -1, 850, 851, 961, -1, 853, 850, 961, -1, 856, 853, 961, -1, 839, 856, 961, -1, 860, 857, 962, -1, 862, 860, 962, -1, 847, 862, 962, -1, 846, 847, 962, -1, 845, 846, 962, -1, 844, 845, 962, -1, 843, 844, 962, -1, 842, 843, 962, -1, 841, 842, 962, -1, 806, 841, 962, -1, 840, 801, 962, -1, 848, 840, 962, -1, 849, 848, 962, -1, 852, 849, 962, -1, 854, 852, 962, -1, 857, 854, 962, -1, 903, 902, 962, -1, 904, 903, 962, -1, 801, 904, 962, -1, 889, 806, 962, -1, 890, 889, 962, -1, 892, 890, 962, -1, 894, 892, 962, -1, 896, 894, 962, -1, 898, 896, 962, -1, 900, 898, 962, -1, 902, 900, 962, -1, 912, 910, 963, -1, 914, 912, 963, -1, 916, 914, 963, -1, 918, 916, 963, -1, 920, 918, 963, -1, 922, 920, 963, -1, 829, 922, 963, -1, 934, 832, 963, -1, 936, 934, 963, -1, 910, 936, 963, -1, 872, 873, 963, -1, 876, 872, 963, -1, 879, 876, 963, -1, 881, 879, 963, -1, 832, 881, 963, -1, 880, 829, 963, -1, 877, 880, 963, -1, 874, 877, 963, -1, 869, 874, 963, -1, 868, 869, 963, -1, 867, 868, 963, -1, 866, 867, 963, -1, 865, 866, 963, -1, 886, 865, 963, -1, 887, 886, 963, -1, 873, 887, 963, -1, 964, 965, 966, -1, 964, 966, 967, -1, 964, 967, 968, -1, 964, 968, 969, -1, 964, 969, 970, -1, 964, 970, 971, -1, 972, 973, 965, -1, 974, 975, 976, -1, 974, 977, 978, -1, 974, 978, 979, -1, 974, 979, 980, -1, 974, 980, 981, -1, 974, 981, 982, -1, 974, 982, 983, -1, 974, 983, 984, -1, 974, 984, 985, -1, 974, 985, 972, -1, 974, 971, 986, -1, 974, 986, 987, -1, 974, 987, 988, -1, 974, 988, 989, -1, 974, 989, 990, -1, 974, 990, 977, -1, 974, 976, 971, -1, 974, 972, 991, -1, 974, 991, 975, -1, 973, 992, 993, -1, 992, 994, 993, -1, 995, 973, 996, -1, 973, 993, 996, -1, 997, 995, 998, -1, 995, 996, 998, -1, 999, 997, 1000, -1, 997, 998, 1000, -1, 1001, 999, 1002, -1, 999, 1000, 1002, -1, 1003, 1001, 1004, -1, 1001, 1002, 1004, -1, 1005, 1003, 1006, -1, 1003, 1004, 1006, -1, 971, 1005, 1007, -1, 1005, 1006, 1007, -1, 1008, 1009, 1010, -1, 1011, 1012, 1013, -1, 1014, 1012, 1011, -1, 1014, 1011, 1015, -1, 1016, 1014, 1015, -1, 1017, 1016, 1018, -1, 1017, 1014, 1016, -1, 1019, 1017, 1018, -1, 1020, 1019, 1021, -1, 1020, 1017, 1019, -1, 1022, 1021, 1023, -1, 1022, 1020, 1021, -1, 986, 971, 1024, -1, 1025, 986, 1024, -1, 987, 1025, 1026, -1, 987, 986, 1025, -1, 988, 1026, 1027, -1, 988, 987, 1026, -1, 989, 988, 1027, -1, 990, 1027, 1028, -1, 990, 989, 1027, -1, 1029, 1030, 1031, -1, 1032, 1033, 1030, -1, 1032, 1030, 1029, -1, 1034, 1031, 1008, -1, 1034, 1029, 1031, -1, 1035, 1036, 1033, -1, 1035, 1033, 1032, -1, 1037, 1008, 1010, -1, 1037, 1034, 1008, -1, 1038, 984, 983, -1, 1038, 985, 984, -1, 1038, 972, 985, -1, 1039, 1040, 1036, -1, 1039, 1036, 1035, -1, 1041, 1037, 1010, -1, 982, 1038, 983, -1, 1042, 1043, 1040, -1, 1042, 1040, 1039, -1, 1044, 1045, 1043, -1, 1044, 1043, 1042, -1, 1046, 1023, 1045, -1, 1046, 1045, 1044, -1, 1047, 981, 980, -1, 1047, 982, 981, -1, 1047, 1038, 982, -1, 1048, 1049, 1050, -1, 1051, 1047, 980, -1, 1051, 980, 979, -1, 1052, 1051, 979, -1, 1053, 1054, 1055, -1, 1056, 1054, 1053, -1, 1056, 1057, 1054, -1, 1058, 1057, 1056, -1, 1059, 1057, 1058, -1, 1060, 1057, 1059, -1, 1061, 1060, 1059, -1, 1062, 1063, 1064, -1, 1062, 1064, 1065, -1, 1066, 1060, 1061, -1, 1067, 1062, 1065, -1, 1067, 1065, 1068, -1, 1069, 1060, 1066, -1, 1070, 1067, 1068, -1, 1070, 1068, 1071, -1, 1072, 1060, 1069, -1, 1073, 1071, 1048, -1, 1073, 1070, 1071, -1, 1074, 1060, 1072, -1, 1075, 1074, 1072, -1, 1076, 1077, 1073, -1, 1050, 1076, 1048, -1, 1048, 1076, 1073, -1, 1078, 1023, 1046, -1, 1078, 1079, 1022, -1, 1078, 1080, 1079, -1, 1078, 1081, 1080, -1, 1078, 1028, 1081, -1, 1078, 977, 990, -1, 1078, 978, 977, -1, 1078, 979, 978, -1, 1078, 1053, 1055, -1, 1078, 1022, 1023, -1, 1078, 1055, 1082, -1, 1078, 1082, 1083, -1, 1078, 1083, 1052, -1, 1078, 1084, 1053, -1, 1078, 1085, 1084, -1, 1078, 1086, 1085, -1, 1078, 1064, 1063, -1, 1078, 1063, 1086, -1, 1078, 990, 1028, -1, 1078, 1087, 1064, -1, 1078, 1088, 1087, -1, 1078, 1089, 1088, -1, 1078, 1046, 1089, -1, 1078, 1052, 979, -1, 1090, 1091, 1092, -1, 1093, 1090, 1092, -1, 1094, 1093, 1092, -1, 1095, 1094, 1092, -1, 1096, 1095, 1092, -1, 1097, 1096, 1092, -1, 1098, 1097, 1092, -1, 1099, 1098, 1092, -1, 994, 1099, 1092, -1, 1100, 1101, 1092, -1, 1102, 1100, 1092, -1, 1103, 1102, 1092, -1, 1104, 1103, 1092, -1, 1105, 1104, 1092, -1, 1091, 1105, 1092, -1, 1106, 1007, 1092, -1, 1107, 1106, 1092, -1, 1101, 1107, 1092, -1, 993, 994, 1092, -1, 996, 993, 1092, -1, 998, 996, 1092, -1, 1000, 998, 1092, -1, 1002, 1000, 1092, -1, 1004, 1002, 1092, -1, 1006, 1004, 1092, -1, 1007, 1006, 1092, -1, 1108, 971, 1109, -1, 1109, 971, 1007, -1, 1110, 1111, 1112, -1, 992, 1113, 994, -1, 1113, 1112, 994, -1, 1114, 1115, 1116, -1, 1117, 1114, 1116, -1, 1118, 1117, 1119, -1, 1117, 1116, 1119, -1, 1120, 1118, 1121, -1, 1118, 1119, 1121, -1, 1122, 1120, 1123, -1, 1120, 1121, 1123, -1, 1082, 1122, 1124, -1, 1122, 1123, 1124, -1, 1125, 1082, 1126, -1, 1082, 1124, 1126, -1, 1127, 1125, 1128, -1, 1125, 1126, 1128, -1, 1051, 1127, 1129, -1, 1127, 1128, 1129, -1, 1110, 1051, 1111, -1, 1051, 1129, 1111, -1, 1113, 1110, 1112, -1, 1130, 1131, 1132, -1, 1133, 1130, 1132, -1, 1134, 1013, 1009, -1, 1134, 1040, 1043, -1, 1134, 1043, 1045, -1, 1134, 1045, 1023, -1, 1134, 1023, 1021, -1, 1134, 1021, 1019, -1, 1134, 1019, 1018, -1, 1134, 1018, 1016, -1, 1134, 1016, 1015, -1, 1134, 1015, 1011, -1, 1134, 1011, 1013, -1, 1134, 1009, 1008, -1, 1134, 1008, 1031, -1, 1134, 1031, 1030, -1, 1134, 1030, 1033, -1, 1134, 1033, 1036, -1, 1134, 1036, 1040, -1, 1013, 1135, 1136, -1, 1137, 1013, 1136, -1, 1138, 1136, 1135, -1, 1138, 1135, 1139, -1, 1140, 1139, 1141, -1, 1140, 1138, 1139, -1, 1142, 1141, 1143, -1, 1142, 1140, 1141, -1, 1022, 1142, 1143, -1, 1144, 1142, 1022, -1, 1145, 1144, 1022, -1, 1146, 1144, 1145, -1, 1147, 1146, 1145, -1, 1148, 1146, 1147, -1, 1149, 1148, 1147, -1, 1150, 1148, 1149, -1, 1151, 1150, 1149, -1, 1152, 1150, 1151, -1, 1027, 1152, 1151, -1, 1153, 1152, 1027, -1, 1154, 1153, 1027, -1, 1155, 1153, 1154, -1, 1156, 1155, 1154, -1, 1157, 1155, 1156, -1, 1108, 1157, 1156, -1, 1109, 1157, 1108, -1, 1114, 1158, 1115, -1, 1115, 1158, 1159, -1, 1160, 1072, 1069, -1, 1160, 1069, 1066, -1, 1160, 1066, 1061, -1, 1160, 1061, 1059, -1, 1160, 1059, 1058, -1, 1160, 1058, 1056, -1, 1160, 1056, 1053, -1, 1160, 1053, 1084, -1, 1160, 1084, 1085, -1, 1160, 1085, 1086, -1, 1160, 1086, 1063, -1, 1160, 1063, 1062, -1, 1160, 1062, 1067, -1, 1160, 1067, 1070, -1, 1160, 1070, 1073, -1, 1160, 1073, 1077, -1, 1160, 1077, 1075, -1, 1160, 1075, 1072, -1, 1161, 1162, 1163, -1, 1163, 1162, 1164, -1, 1164, 1162, 1165, -1, 1164, 1165, 1166, -1, 1165, 1167, 1168, -1, 1166, 1165, 1168, -1, 1169, 1065, 1064, -1, 1169, 1064, 1087, -1, 1169, 1087, 1088, -1, 1169, 1088, 1089, -1, 1169, 1089, 1046, -1, 1169, 1046, 1044, -1, 1169, 1044, 1042, -1, 1169, 1042, 1039, -1, 1169, 1039, 1035, -1, 1169, 1035, 1032, -1, 1169, 1032, 1029, -1, 1169, 1029, 1034, -1, 1169, 1034, 1037, -1, 1169, 1037, 1041, -1, 1169, 1041, 1049, -1, 1169, 1049, 1048, -1, 1169, 1048, 1071, -1, 1169, 1071, 1068, -1, 1169, 1068, 1065, -1, 1170, 1130, 1133, -1, 1171, 1170, 1133, -1, 1133, 1132, 1172, -1, 1137, 1136, 1173, -1, 1173, 1136, 1138, -1, 1174, 1173, 1138, -1, 1174, 1138, 1175, -1, 1175, 1138, 1140, -1, 1176, 1175, 1140, -1, 1176, 1140, 1177, -1, 1177, 1140, 1142, -1, 1178, 1177, 1142, -1, 1178, 1142, 1144, -1, 1179, 1178, 1144, -1, 1109, 1007, 1106, -1, 1109, 1106, 1107, -1, 1109, 1107, 1101, -1, 1109, 1101, 1157, -1, 1157, 1101, 1100, -1, 1157, 1100, 1102, -1, 1155, 1157, 1102, -1, 1155, 1102, 1103, -1, 1153, 1155, 1103, -1, 1152, 1153, 1104, -1, 1153, 1103, 1104, -1, 1152, 1104, 1105, -1, 1180, 1181, 1182, -1, 1182, 1181, 1183, -1, 1181, 1184, 1183, -1, 1172, 1180, 1185, -1, 1180, 1182, 1185, -1, 1184, 1186, 1187, -1, 1183, 1184, 1187, -1, 1133, 1172, 1188, -1, 1172, 1185, 1188, -1, 1097, 1098, 1112, -1, 1098, 1099, 1112, -1, 1099, 994, 1112, -1, 1186, 1189, 1190, -1, 1187, 1186, 1190, -1, 1133, 1188, 1171, -1, 1097, 1112, 1096, -1, 1190, 1189, 1191, -1, 1189, 1192, 1191, -1, 1191, 1192, 1193, -1, 1192, 1194, 1193, -1, 1193, 1194, 1195, -1, 1194, 1179, 1195, -1, 1094, 1095, 1111, -1, 1095, 1096, 1111, -1, 1096, 1112, 1111, -1, 1166, 1168, 1196, -1, 1094, 1111, 1129, -1, 1093, 1094, 1129, -1, 1093, 1129, 1128, -1, 1123, 1121, 1197, -1, 1197, 1121, 1198, -1, 1121, 1119, 1198, -1, 1198, 1119, 1199, -1, 1199, 1119, 1200, -1, 1200, 1119, 1116, -1, 1200, 1116, 1201, -1, 1202, 1203, 1204, -1, 1203, 1205, 1204, -1, 1201, 1116, 1206, -1, 1207, 1202, 1208, -1, 1202, 1204, 1208, -1, 1206, 1116, 1209, -1, 1210, 1207, 1211, -1, 1207, 1208, 1211, -1, 1209, 1116, 1212, -1, 1196, 1210, 1213, -1, 1210, 1211, 1213, -1, 1212, 1116, 1115, -1, 1212, 1115, 1159, -1, 1213, 1163, 1164, -1, 1196, 1164, 1166, -1, 1213, 1164, 1196, -1, 1195, 1179, 1214, -1, 1203, 1215, 1214, -1, 1215, 1216, 1214, -1, 1216, 1217, 1214, -1, 1217, 1195, 1214, -1, 1093, 1128, 1214, -1, 1205, 1203, 1214, -1, 1179, 1144, 1214, -1, 1197, 1218, 1214, -1, 1218, 1219, 1214, -1, 1219, 1205, 1214, -1, 1123, 1197, 1214, -1, 1124, 1123, 1214, -1, 1126, 1124, 1214, -1, 1128, 1126, 1214, -1, 1144, 1146, 1214, -1, 1146, 1148, 1214, -1, 1148, 1150, 1214, -1, 1150, 1152, 1214, -1, 1091, 1090, 1214, -1, 1090, 1093, 1214, -1, 1152, 1105, 1214, -1, 1105, 1091, 1214, -1, 1220, 1013, 1221, -1, 1013, 1137, 1221, -1, 1220, 1221, 1222, -1, 1220, 1222, 1223, -1, 1223, 1222, 1224, -1, 1223, 1224, 1225, -1, 1225, 1224, 1226, -1, 1225, 1226, 1227, -1, 1227, 1226, 1228, -1, 1227, 1228, 1229, -1, 1229, 1228, 1230, -1, 1229, 1230, 1231, -1, 1231, 1230, 1232, -1, 1231, 1232, 1233, -1, 1233, 1232, 1234, -1, 1234, 1235, 1131, -1, 1235, 1236, 1131, -1, 1233, 1234, 1131, -1, 1131, 1236, 1132, -1, 1009, 1237, 1238, -1, 1009, 1238, 1239, -1, 1009, 1239, 1233, -1, 1013, 1220, 1223, -1, 1013, 1223, 1240, -1, 1013, 1240, 1237, -1, 1013, 1237, 1009, -1, 1241, 1242, 1243, -1, 1243, 1242, 1244, -1, 1243, 1244, 1245, -1, 1245, 1244, 1246, -1, 1245, 1246, 1247, -1, 1247, 1246, 1248, -1, 1247, 1248, 1249, -1, 1249, 1248, 1250, -1, 1249, 1250, 1251, -1, 1251, 1250, 1252, -1, 1251, 1252, 1253, -1, 1253, 1252, 1254, -1, 1253, 1254, 1158, -1, 1158, 1254, 1159, -1, 1255, 1077, 1256, -1, 1255, 1256, 1257, -1, 1255, 1257, 1258, -1, 1255, 1075, 1077, -1, 1255, 1258, 1259, -1, 1255, 1259, 1260, -1, 1255, 1260, 1261, -1, 1255, 1261, 1262, -1, 1255, 1262, 1263, -1, 1255, 1263, 1264, -1, 1255, 1264, 1075, -1, 1161, 1163, 1265, -1, 1265, 1163, 1266, -1, 1265, 1266, 1267, -1, 1267, 1266, 1268, -1, 1267, 1268, 1241, -1, 1241, 1268, 1242, -1, 1269, 1270, 1271, -1, 1271, 1270, 1272, -1, 1271, 1272, 1273, -1, 1273, 1272, 1274, -1, 1273, 1274, 1167, -1, 1167, 1274, 1168, -1, 1049, 1041, 1275, -1, 1049, 1275, 1276, -1, 1049, 1276, 1277, -1, 1049, 1277, 1278, -1, 1049, 1278, 1279, -1, 1049, 1279, 1280, -1, 1049, 1280, 1281, -1, 1049, 1281, 1282, -1, 1283, 1170, 1284, -1, 1170, 1171, 1284, -1, 1285, 1283, 1286, -1, 1283, 1284, 1286, -1, 1287, 1285, 1288, -1, 1285, 1286, 1288, -1, 1289, 1287, 1290, -1, 1287, 1288, 1290, -1, 1291, 1289, 1292, -1, 1289, 1290, 1292, -1, 1291, 1292, 1269, -1, 1269, 1292, 1270, -1, 1272, 1270, 1293, -1, 1274, 1272, 1293, -1, 1168, 1274, 1293, -1, 1284, 1171, 1293, -1, 1286, 1284, 1293, -1, 1288, 1286, 1293, -1, 1290, 1288, 1293, -1, 1292, 1290, 1293, -1, 1270, 1292, 1293, -1, 1196, 1168, 1293, -1, 1210, 1196, 1293, -1, 1207, 1210, 1293, -1, 1202, 1207, 1293, -1, 1203, 1202, 1293, -1, 1215, 1203, 1293, -1, 1216, 1215, 1293, -1, 1217, 1216, 1293, -1, 1195, 1217, 1293, -1, 1193, 1195, 1293, -1, 1191, 1193, 1293, -1, 1190, 1191, 1293, -1, 1187, 1190, 1293, -1, 1183, 1187, 1293, -1, 1182, 1183, 1293, -1, 1185, 1182, 1293, -1, 1188, 1185, 1293, -1, 1171, 1188, 1293, -1, 1192, 1189, 1294, -1, 1194, 1192, 1294, -1, 1179, 1194, 1294, -1, 1178, 1179, 1294, -1, 1177, 1178, 1294, -1, 1176, 1177, 1294, -1, 1175, 1176, 1294, -1, 1174, 1175, 1294, -1, 1173, 1174, 1294, -1, 1137, 1173, 1294, -1, 1172, 1132, 1294, -1, 1180, 1172, 1294, -1, 1181, 1180, 1294, -1, 1184, 1181, 1294, -1, 1186, 1184, 1294, -1, 1189, 1186, 1294, -1, 1235, 1234, 1294, -1, 1236, 1235, 1294, -1, 1132, 1236, 1294, -1, 1221, 1137, 1294, -1, 1222, 1221, 1294, -1, 1224, 1222, 1294, -1, 1226, 1224, 1294, -1, 1228, 1226, 1294, -1, 1230, 1228, 1294, -1, 1232, 1230, 1294, -1, 1234, 1232, 1294, -1, 1244, 1242, 1295, -1, 1246, 1244, 1295, -1, 1248, 1246, 1295, -1, 1250, 1248, 1295, -1, 1252, 1250, 1295, -1, 1254, 1252, 1295, -1, 1159, 1254, 1295, -1, 1266, 1163, 1295, -1, 1268, 1266, 1295, -1, 1242, 1268, 1295, -1, 1204, 1205, 1295, -1, 1208, 1204, 1295, -1, 1211, 1208, 1295, -1, 1213, 1211, 1295, -1, 1163, 1213, 1295, -1, 1212, 1159, 1295, -1, 1209, 1212, 1295, -1, 1206, 1209, 1295, -1, 1201, 1206, 1295, -1, 1200, 1201, 1295, -1, 1199, 1200, 1295, -1, 1198, 1199, 1295, -1, 1197, 1198, 1295, -1, 1218, 1197, 1295, -1, 1219, 1218, 1295, -1, 1205, 1219, 1295, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -2.000 17.250 59.227 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -2.000 17.250 -59.230 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -2.000 -41.979 -0.001 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -2.000 76.479 -0.001 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -61.229 17.250 -0.001 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 57.229 17.250 -0.001 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/L_SHOULDER_R.wrl000066400000000000000000002224371207742442300235560ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 37.000 36.299 65.500 center 0.000 0.000 0.000 #translation 0.500 -2.150 22.750 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.50 0.50 0.50 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -16.500 3.975 0.448, -16.500 4.000 -0.000, -16.500 0.000 0.000, -16.500 3.900 0.890, -16.500 3.776 1.321, -16.500 3.604 1.736, -16.500 3.387 2.128, -16.500 3.127 2.494, -16.500 2.828 2.828, -16.500 2.494 3.127, -16.500 2.128 3.387, -16.500 1.736 3.604, -16.500 1.321 3.776, -16.500 0.890 3.900, -16.500 0.448 3.975, -16.500 0.000 4.000, -16.500 -0.448 3.975, -16.500 -0.890 3.900, -16.500 -1.321 3.776, -16.500 -1.736 3.604, -16.500 -2.128 3.387, -16.500 -2.494 3.127, -16.500 -2.828 2.828, -16.500 -3.127 2.494, -16.500 -3.387 2.128, -16.500 -3.604 1.736, -16.500 -3.776 1.321, -16.500 -3.900 0.890, -16.500 -3.975 0.448, -16.500 -4.000 0.000, -16.500 -3.975 -0.448, -16.500 -3.900 -0.890, -16.500 -3.776 -1.321, -16.500 -3.604 -1.736, -16.500 -3.387 -2.128, -16.500 -3.127 -2.494, -16.500 -2.828 -2.828, -16.500 -2.494 -3.127, -16.500 -2.128 -3.387, -16.500 -1.736 -3.604, -16.500 -1.321 -3.776, -16.500 -0.890 -3.900, -16.500 -0.448 -3.975, -16.500 0.000 -4.000, -16.500 0.448 -3.975, -16.500 0.890 -3.900, -16.500 1.321 -3.776, -16.500 1.736 -3.604, -16.500 2.128 -3.387, -16.500 2.494 -3.127, -16.500 2.828 -2.828, -16.500 3.127 -2.494, -16.500 3.387 -2.128, -16.500 3.604 -1.736, -16.500 3.776 -1.321, -16.500 3.900 -0.890, -16.500 3.975 -0.448, -15.550 4.000 -0.000, -16.500 4.000 -0.000, -15.550 3.864 1.035, -15.550 3.464 2.000, -15.550 2.828 2.828, -15.550 2.000 3.464, -15.550 1.035 3.864, -15.550 0.000 4.000, -16.500 -0.448 3.975, -15.550 -1.035 3.864, -15.550 -2.000 3.464, -15.550 -2.828 2.828, -15.550 -3.464 2.000, -15.550 -3.864 1.035, -15.550 -4.000 0.000, -15.550 -3.864 -1.035, -15.550 -3.464 -2.000, -15.550 -2.828 -2.828, -15.550 -2.000 -3.464, -15.550 -1.035 -3.864, -15.550 0.000 -4.000, -15.550 1.035 -3.864, -16.500 0.890 -3.900, -15.550 2.000 -3.464, -15.550 2.828 -2.828, -15.550 3.464 -2.000, -15.550 3.864 -1.035, -15.550 -11.250 10.000, -15.550 11.250 -29.000, -15.550 -11.250 -29.000, -15.550 11.250 10.000, -15.550 0.000 -9.500, -15.550 -9.925 -29.000, -15.550 -6.855 -29.000, -15.550 6.855 -29.000, -15.550 9.925 -29.000, -14.784 -10.823 -29.000, -14.377 -11.250 -29.000, -15.175 -10.381 -29.000, 9.550 -11.250 -29.000, 9.550 -11.250 10.000, -3.000 -11.250 -9.500, 9.500 -11.250 -29.000, 8.377 -11.250 -29.000, -2.500 -11.250 -29.000, -11.828 -11.250 -29.000, -17.027 7.697 -29.000, -18.113 5.254 -29.000, -16.513 4.678 -29.000, -18.777 2.664 -29.000, -17.102 2.372 -29.000, -19.000 0.000 -29.000, -17.300 0.000 -29.000, -17.102 -2.372 -29.000, -18.777 -2.664 -29.000, -16.513 -4.678 -29.000, -18.113 -5.254 -29.000, -17.027 -7.697 -29.000, -15.550 11.250 10.000, -15.550 -11.250 10.000, 9.550 -11.250 10.000, 9.550 11.250 10.000, -16.425 -0.000 -29.000, -3.000 11.250 -9.500, 9.550 11.250 10.000, 9.550 11.250 -29.000, 9.500 11.250 -29.000, 8.377 11.250 -29.000, -14.377 11.250 -29.000, -2.500 11.250 -29.000, -11.828 11.250 -29.000, -14.784 10.823 -29.000, -15.175 10.381 -29.000, -4.099 -15.962 -29.000, -6.947 -15.505 -29.000, -5.344 -15.827 -31.000, -3.281 -15.998 -31.000, -18.866 -2.065 -31.000, -19.000 0.000 -31.000, -1.214 -15.900 -29.000, -1.214 -15.900 -31.000, -18.467 -4.096 -31.000, -17.809 -6.058 -31.000, -16.903 -7.919 -31.000, -15.765 -9.647 -31.000, -14.413 -11.214 -31.000, -12.870 -12.593 -31.000, -12.172 -13.110 -29.000, -11.162 -13.762 -31.000, -9.668 -14.544 -29.000, -9.317 -14.700 -31.000, -7.367 -15.393 -31.000, 9.550 -5.418 -5.886, 9.550 -6.033 -5.254, 9.550 -4.738 -6.446, 9.550 -6.574 -4.558, 9.550 -4.000 -6.928, 9.550 -7.036 -3.808, 9.550 -3.214 -7.326, 9.550 -7.412 -3.010, 9.550 -7.698 -2.177, 9.550 -7.891 -1.317, 9.550 3.612 -7.138, 9.550 4.376 -6.697, 9.550 -7.036 3.808, 9.550 -6.574 4.558, 9.550 -7.412 3.010, 9.550 -7.698 2.177, 9.550 -7.891 1.317, 9.550 -7.988 0.441, 9.550 -7.988 -0.441, 9.550 5.086 -6.175, 9.550 -6.033 5.254, 9.550 5.734 -5.578, 9.550 -5.418 5.886, 9.550 6.313 -4.914, 9.550 -4.738 6.446, 9.550 6.815 -4.189, 9.550 -4.000 6.928, 9.550 7.235 -3.414, 9.550 -3.214 7.326, 9.550 -2.388 7.635, 9.550 -1.534 7.852, 9.550 -0.661 7.973, 9.550 0.220 7.997, 9.550 7.951 0.880, 9.550 8.000 -0.000, 9.550 7.806 1.749, 9.550 7.567 2.598, 9.550 7.235 3.414, 9.550 6.815 4.189, 9.550 6.313 4.914, 9.550 5.734 5.578, 9.550 5.086 6.175, 9.550 4.376 6.697, 9.550 3.612 7.138, 9.550 2.805 7.492, 9.550 1.964 7.755, 9.550 1.099 7.924, 9.550 7.951 -0.880, 9.550 7.806 -1.749, 9.550 7.567 -2.598, 9.550 -0.000 -9.500, 9.550 -2.388 -7.635, 9.550 -1.534 -7.852, 9.550 -0.661 -7.973, 9.550 0.220 -7.997, 9.550 1.099 -7.924, 9.550 1.964 -7.755, 9.550 2.805 -7.492, 9.500 -9.987 -29.000, 9.500 9.987 -29.000, 9.525 -0.000 -29.000, 9.500 -14.200 -29.000, 7.147 -12.371 -29.000, 5.808 -13.358 -29.000, 4.373 -14.200 -29.000, -2.500 -14.200 -29.000, -1.312 -14.200 -29.000, 2.939 -12.725 -29.000, -3.000 -14.300 -29.000, -5.368 -14.103 -29.000, -3.000 -15.900 -29.000, -7.670 -13.516 -29.000, -9.843 -12.556 -29.000, -7.796 -13.606 -29.000, -3.000 -14.200 -29.000, -7.164 -12.775 -29.000, -18.455 4.141 -31.000, -17.782 6.123 -31.000, -5.088 15.863 -31.000, -3.000 16.000 -29.000, -3.000 16.000 -31.000, -18.863 2.088 -31.000, -5.688 15.773 -29.000, -7.141 15.455 -31.000, -8.299 15.097 -29.000, -9.123 14.782 -31.000, -10.760 13.992 -29.000, -11.000 13.856 -31.000, -12.740 12.694 -31.000, -13.000 12.490 -29.000, -13.475 12.095 -29.000, -14.314 11.314 -31.000, -13.934 11.681 -29.000, -15.694 9.740 -31.000, -16.856 8.000 -31.000, 9.500 14.300 -29.000, 7.090 12.417 -29.000, 5.685 13.438 -29.000, 4.177 14.300 -29.000, -0.500 11.300 -29.000, -2.500 11.300 -29.000, 2.939 12.775 -29.000, -0.500 14.300 -29.000, -13.000 11.300 -29.000, -11.764 11.300 -29.000, -11.785 11.283 -29.000, -11.806 11.267 -29.000, -7.164 11.275 -29.000, -5.344 -15.827 -31.000, -5.182 -15.851 -53.368, -4.588 -15.921 -54.186, -5.773 -15.758 -52.554, -12.870 -12.593 -31.000, -14.413 -11.214 -31.000, -13.685 -11.909 -50.134, -6.436 -15.627 -51.605, -7.367 -15.393 -31.000, -3.281 -15.998 -31.000, -3.633 -15.987 -55.500, -2.825 -15.999 -55.500, -2.018 -15.970 -55.500, -11.162 -13.762 -31.000, -11.499 -13.556 -49.521, -10.331 -14.222 -49.537, -18.866 -2.065 -31.000, -19.000 0.000 -55.500, -18.774 -2.683 -55.500, -19.000 0.000 -31.000, -1.214 -15.900 -31.000, -1.214 -15.900 -55.500, -18.467 -4.096 -31.000, -18.101 -5.289 -55.500, -16.971 -7.797 -54.915, -17.000 -7.746 -55.500, -9.317 -14.700 -31.000, -9.264 -14.723 -49.757, -8.216 -15.126 -50.185, -17.809 -6.058 -31.000, -16.886 -7.948 -54.338, -16.903 -7.919 -31.000, -16.632 -8.376 -53.432, -16.188 -9.059 -52.487, -15.765 -9.647 -31.000, -15.554 -9.920 -51.593, -14.678 -10.937 -50.760, -7.255 -15.424 -50.813, -12.615 -12.789 -49.721, 6.573 -15.900 -54.980, 8.000 -15.900 -55.500, 8.000 -15.900 -55.367, 9.500 -15.900 -35.000, 6.017 -15.900 -39.246, 7.331 -15.900 -38.781, 8.703 -15.900 -38.537, 10.096 -15.900 -38.521, 11.473 -15.900 -38.732, 12.797 -15.900 -39.165, 14.033 -15.900 -39.809, 5.234 -15.900 -54.352, 4.025 -15.900 -53.502, 2.981 -15.900 -52.454, 9.500 -15.900 -29.000, 1.454 -15.900 -44.260, 2.009 -15.900 -42.983, 2.766 -15.900 -41.813, 3.704 -15.900 -40.783, 4.798 -15.900 -39.919, 2.134 -15.900 -51.242, -1.214 -15.900 -29.000, 1.000 -15.900 -47.000, 1.114 -15.900 -45.611, 15.146 -15.900 -40.646, 1.511 -15.900 -49.901, 1.129 -15.900 -48.473, 10.550 7.951 0.880, 10.550 8.000 -0.000, 10.550 7.806 1.749, 10.550 7.567 2.598, 10.550 7.235 3.414, 10.550 6.815 4.189, 10.550 6.313 4.914, 10.550 5.734 5.578, 10.550 5.086 6.175, 10.550 4.376 6.697, 10.550 3.612 7.138, 10.550 2.805 7.492, 10.550 1.964 7.755, 10.550 1.099 7.924, 10.550 0.220 7.997, 10.550 -0.661 7.973, 10.550 -1.534 7.852, 10.550 -2.388 7.635, 10.550 -3.214 7.326, 10.550 -4.000 6.928, 10.550 -4.738 6.446, 10.550 -5.418 5.886, 10.550 -6.033 5.254, 10.550 -6.574 4.558, 10.550 -7.036 3.808, 10.550 -7.412 3.010, 10.550 -7.698 2.177, 10.550 -7.891 1.317, 10.550 -7.988 0.441, 10.550 -7.988 -0.441, 10.550 -7.891 -1.317, 10.550 -7.698 -2.177, 10.550 -7.412 -3.010, 10.550 -7.036 -3.808, 10.550 -6.574 -4.558, 10.550 -6.033 -5.254, 10.550 -5.418 -5.886, 10.550 -4.738 -6.446, 10.550 -4.000 -6.928, 10.550 -3.214 -7.326, 10.550 -2.388 -7.635, 10.550 -1.534 -7.852, 10.550 -0.661 -7.973, 10.550 0.220 -7.997, 10.550 1.099 -7.924, 10.550 1.964 -7.755, 10.550 2.805 -7.492, 10.550 3.612 -7.138, 10.550 4.376 -6.697, 10.550 5.086 -6.175, 10.550 5.734 -5.578, 10.550 6.313 -4.914, 10.550 6.815 -4.189, 10.550 7.235 -3.414, 10.550 7.567 -2.598, 10.550 7.806 -1.749, 10.550 7.951 -0.880, 9.500 -9.987 -31.000, 9.500 -12.094 -30.000, 9.500 -14.200 -31.000, 9.500 9.987 -31.000, 9.500 12.144 -30.000, 9.500 14.300 -31.000, 6.034 -14.600 -29.000, 2.568 -15.000 -29.000, 9.500 -15.000 -29.000, 3.180 -14.758 -29.000, 3.782 -14.491 -29.000, 4.373 -14.200 -29.000, 9.500 -14.200 -29.000, -2.500 -15.000 -29.000, -2.103 -14.272 -29.000, -2.500 -14.291 -29.000, -1.707 -14.241 -29.000, -1.312 -14.200 -29.000, 0.936 -14.600 -29.000, -2.833 -14.299 -29.000, -2.667 -14.296 -29.000, 1.333 -15.402 -29.000, 0.070 -15.703 -29.000, -2.750 -14.250 -29.000, 8.000 16.000 -55.367, 8.000 16.000 -55.500, 6.424 16.000 -54.924, 7.810 16.000 -38.670, 6.304 16.000 -39.124, 9.500 16.000 -35.000, 9.374 16.000 -38.501, 10.942 16.000 -38.623, 12.461 16.000 -39.033, 13.879 16.000 -39.715, 3.668 16.000 -40.816, 2.538 16.000 -42.124, -2.500 16.000 -31.000, 4.907 16.000 -39.848, 9.500 16.000 -31.000, 2.538 16.000 -51.876, 3.668 16.000 -53.184, -3.000 16.000 -55.500, 4.962 16.000 -54.187, 1.695 16.000 -43.633, 1.176 16.000 -45.281, 1.695 16.000 -50.367, -2.500 16.000 -29.000, 1.000 16.000 -47.000, 15.146 16.000 -40.646, 1.176 16.000 -48.719, -17.782 6.123 -31.000, -16.856 8.000 -31.000, -16.958 7.822 -54.791, -18.101 5.289 -55.500, -18.455 4.141 -31.000, -18.774 2.683 -55.500, -11.000 13.856 -31.000, -11.081 13.809 -49.501, -12.216 13.079 -49.624, -9.993 14.391 -49.585, -9.123 14.782 -31.000, -18.863 2.088 -31.000, -4.230 15.953 -54.678, -3.422 15.994 -55.500, -3.633 15.987 -55.500, -12.740 12.694 -31.000, -13.344 12.206 -49.977, -3.211 15.999 -55.500, -14.314 11.314 -31.000, -14.391 11.236 -50.550, -15.290 10.244 -51.306, -5.088 15.863 -31.000, -4.826 15.896 -53.858, -5.773 15.758 -52.554, -6.283 15.660 -51.792, -7.141 15.455 -31.000, -6.950 15.505 -51.073, -7.859 15.244 -50.388, -15.694 9.740 -31.000, -16.051 9.256 -52.261, -17.000 7.746 -55.500, -16.833 8.040 -54.094, -8.877 14.882 -49.888, -16.543 8.520 -53.203, -0.500 16.000 -29.000, -0.500 20.300 -29.000, -13.000 20.300 -29.000, -6.750 16.395 -29.000, -0.500 15.803 -29.000, 1.108 15.464 -29.000, 4.500 15.150 -29.000, 2.672 14.961 -29.000, 9.500 16.000 -29.000, -0.500 11.300 -2.500, -6.750 11.300 -15.750, -13.000 11.300 -2.500, -0.500 15.800 -15.750, -0.500 20.300 -2.500, -13.000 15.800 -15.750, -13.000 20.300 -2.500, -5.183 -14.132 -53.367, -5.773 -14.029 -52.554, -4.703 -15.008 -54.027, -3.633 -14.286 -55.500, -4.589 -14.211 -54.184, -3.422 -14.294 -55.500, -3.211 -14.298 -55.500, -3.000 -15.900 -55.500, -3.000 -14.300 -55.500, -11.455 -11.533 -49.517, -10.309 -12.291 -49.540, -17.000 -2.914 -55.500, -16.996 -2.931 -55.292, -9.245 -12.864 -49.762, -16.986 -2.982 -55.086, -16.955 -3.121 -54.768, -8.209 -13.317 -50.189, -16.874 -3.463 -54.279, -7.253 -13.653 -50.814, -6.435 -13.881 -51.607, -16.618 -4.364 -53.393, -16.133 -5.659 -52.393, -15.411 -7.103 -51.433, -14.546 -8.437 -50.660, -13.579 -9.621 -50.083, -12.548 -10.646 -49.703, -17.000 2.914 -55.500, -17.166 1.950 -55.500, -17.267 0.977 -55.500, -17.267 -0.977 -55.500, -17.166 -1.950 -55.500, -17.300 0.000 -55.500, -18.000 -0.000 -55.500, 8.000 -14.200 -55.500, 4.373 -14.200 -55.500, 0.713 -15.563 -55.500, 2.585 -14.994 -55.500, 3.393 -15.050 -55.500, 8.000 -14.200 -55.367, 4.143 -15.450 -29.000, 15.000 -15.900 -47.500, 17.998 -15.900 -47.167, 18.000 -15.900 -47.000, 17.993 -15.900 -47.333, 17.985 -15.900 -47.500, 17.932 -15.900 -45.926, 17.847 -15.900 -45.394, 17.983 -15.900 -46.462, 13.151 -15.900 -48.634, 15.000 -15.900 -49.500, 12.622 -15.900 -49.500, 13.445 -15.900 -47.662, 13.484 -15.900 -46.648, 13.267 -15.900 -45.656, 8.000 -15.900 -50.708, 16.095 -15.900 -41.638, 12.808 -15.900 -44.751, 16.016 -15.900 -41.542, 15.936 -15.900 -41.447, 15.854 -15.900 -41.354, 6.982 -15.900 -50.108, 12.135 -15.900 -43.991, 6.183 -15.900 -49.236, 11.293 -15.900 -43.424, 5.675 -15.900 -48.169, 10.335 -15.900 -43.088, 5.500 -15.900 -47.000, 9.324 -15.900 -43.004, 8.323 -15.900 -43.177, 5.629 -15.900 -45.993, 6.007 -15.900 -45.051, 7.399 -15.900 -43.596, 6.610 -15.900 -44.235, 9.500 -15.000 -31.000, 9.500 -15.050 -32.000, 9.500 -14.200 -35.000, 9.500 -14.200 -31.000, 15.146 -14.200 -40.646, 10.550 -0.663 -0.749, 10.550 -0.749 -0.663, 10.550 -0.568 -0.823, 10.550 -0.823 -0.568, 10.550 -0.885 -0.465, 10.550 -0.465 -0.885, 10.550 -0.355 -0.935, 10.550 -0.935 -0.355, 10.550 -0.239 -0.971, 10.550 -0.971 -0.239, 10.550 -0.121 -0.993, 10.550 -0.993 -0.121, 10.550 -1.000 0.000, 10.550 -0.000 -1.000, 10.550 -0.993 0.121, 10.550 0.121 -0.993, 10.550 0.239 -0.971, 10.550 -0.971 0.239, 10.550 -7.698 2.177, 10.550 0.355 -0.935, 10.550 -0.935 0.355, 10.550 -7.412 3.010, 10.550 0.465 -0.885, 10.550 4.376 -6.697, 10.550 -0.885 0.465, 10.550 0.568 -0.823, 10.550 -0.823 0.568, 10.550 0.663 -0.749, 10.550 -0.749 0.663, 10.550 0.749 -0.663, 10.550 -0.663 0.749, 10.550 0.823 -0.568, 10.550 -4.738 6.446, 10.550 -0.568 0.823, 10.550 0.885 -0.465, 10.550 -4.000 6.928, 10.550 -0.465 0.885, 10.550 0.935 -0.355, 10.550 -3.214 7.326, 10.550 -0.355 0.935, 10.550 0.971 -0.239, 10.550 -2.388 7.635, 10.550 -0.239 0.971, 10.550 0.993 -0.121, 10.550 -1.534 7.852, 10.550 -0.121 0.993, 10.550 1.000 -0.000, 10.550 -0.661 7.973, 10.550 -0.000 1.000, 10.550 0.220 7.997, 10.550 0.121 0.993, 10.550 0.993 0.121, 10.550 1.099 7.924, 10.550 0.239 0.971, 10.550 0.971 0.239, 10.550 1.964 7.755, 10.550 0.355 0.935, 10.550 7.567 2.598, 10.550 0.935 0.355, 10.550 2.805 7.492, 10.550 7.235 3.414, 10.550 0.885 0.465, 10.550 3.612 7.138, 10.550 0.465 0.885, 10.550 6.815 4.189, 10.550 0.823 0.568, 10.550 4.376 6.697, 10.550 0.568 0.823, 10.550 6.313 4.914, 10.550 0.749 0.663, 10.550 5.086 6.175, 10.550 0.663 0.749, 10.550 5.734 5.578, 7.993 -11.626 -31.000, 9.500 -9.987 -31.000, 6.273 -13.039 -31.000, 4.373 -14.200 -31.000, -1.312 -14.200 -31.000, 9.500 9.987 -31.000, 3.500 0.050 -31.000, -2.500 -14.200 -31.000, -2.500 14.300 -31.000, 4.177 14.300 -31.000, 7.939 11.676 -31.000, 6.153 13.124 -31.000, 9.500 14.300 -31.000, 9.500 16.000 -31.000, 9.500 14.300 -29.000, 9.500 16.000 -29.000, 15.000 16.000 -49.500, 13.830 16.000 -49.500, 14.285 16.000 -48.451, 18.000 16.000 -47.000, 17.998 16.000 -47.167, 15.000 16.000 -47.500, 17.993 16.000 -47.333, 17.985 16.000 -47.500, 17.932 16.000 -45.926, 17.983 16.000 -46.462, 14.489 16.000 -47.327, 17.847 16.000 -45.394, 14.433 16.000 -46.186, 8.000 16.000 -55.367, 6.424 16.000 -54.924, 8.000 16.000 -51.770, 14.119 16.000 -45.087, 4.962 16.000 -54.187, 6.853 16.000 -51.242, 16.095 16.000 -41.638, 13.564 16.000 -44.088, 16.016 16.000 -41.542, 15.936 16.000 -41.447, 15.854 16.000 -41.354, 3.668 16.000 -53.184, 5.875 16.000 -50.443, 12.797 16.000 -43.241, 15.146 16.000 -40.646, 2.538 16.000 -51.876, 5.128 16.000 -49.425, 11.857 16.000 -42.590, 13.879 16.000 -39.715, 1.695 16.000 -50.367, 4.659 16.000 -48.253, 10.794 16.000 -42.170, 12.461 16.000 -39.033, 4.500 16.000 -47.000, 1.176 16.000 -48.719, 9.664 16.000 -42.003, 10.942 16.000 -38.623, 1.000 16.000 -47.000, 8.525 16.000 -42.096, 9.374 16.000 -38.501, 7.810 16.000 -38.670, 4.631 16.000 -45.865, 1.176 16.000 -45.281, 7.436 16.000 -42.446, 6.304 16.000 -39.124, 5.016 16.000 -44.789, 1.695 16.000 -43.633, 6.456 16.000 -43.033, 4.907 16.000 -39.848, 5.635 16.000 -43.828, 2.538 16.000 -42.124, 3.668 16.000 -40.816, 8.000 14.300 -55.367, 8.000 14.300 -55.500, 8.000 16.000 -55.500, 4.177 14.300 -55.500, 1.883 15.237 -55.500, -0.529 15.808 -55.500, 9.500 14.300 -35.000, 15.146 14.300 -40.646, 9.500 16.000 -35.000, 3.500 16.000 -30.000, -3.633 14.286 -55.500, -4.231 14.247 -54.677, -4.703 15.008 -54.027, -5.773 14.029 -52.554, -4.827 14.183 -53.857, -3.316 15.143 -55.500, -3.000 14.300 -55.500, -3.211 14.298 -55.500, -3.422 14.294 -55.500, -10.076 12.426 -49.572, -8.979 12.990 -49.850, -16.993 2.946 -55.216, -7.981 13.404 -50.315, -7.074 13.708 -50.963, -16.973 3.040 -54.934, -16.927 3.246 -54.565, -6.328 13.907 -51.736, -16.817 3.683 -54.031, -16.569 4.514 -53.267, -16.026 5.901 -52.223, -15.291 7.309 -51.306, -14.387 8.651 -50.547, -13.359 9.858 -49.983, -12.260 10.896 -49.634, -11.138 11.759 -49.502, -0.500 20.300 -2.500, -13.000 20.300 -29.000, -13.000 20.300 -2.500, -0.500 20.300 -29.000, -13.000 11.300 -2.500, -12.365 -10.807 -31.000, -13.807 -9.365 -31.000, -7.029 -13.721 -31.000, -8.940 -13.008 -31.000, -10.731 -12.030 -31.000, -17.300 0.000 -31.000, -5.035 -14.154 -31.000, -3.000 -14.300 -31.000, -17.154 -2.035 -31.000, -16.721 -4.029 -31.000, -16.008 -5.940 -31.000, -15.030 -7.731 -31.000, -1.312 -14.200 -55.500, -2.436 -14.289 -55.500, -1.873 -14.256 -55.500, 0.686 -15.050 -55.500, -10.731 12.030 -31.000, -8.940 13.008 -31.000, -12.365 10.807 -31.000, -16.721 4.029 -31.000, -16.008 5.940 -31.000, -17.154 2.035 -31.000, -13.807 9.365 -31.000, -3.000 14.300 -31.000, -15.030 7.731 -31.000, -5.035 14.154 -31.000, -7.029 13.721 -31.000, 6.713 -14.200 -55.030, 5.494 -14.200 -54.497, 4.373 -14.200 -53.780, 17.780 -14.200 -48.920, 17.985 -14.200 -47.500, 17.780 -15.900 -48.920, 17.340 -14.200 -50.284, 17.340 -15.900 -50.284, 16.676 -14.200 -51.556, 16.676 -15.900 -51.556, 15.808 -14.200 -52.697, 15.808 -15.900 -52.697, 14.760 -15.900 -53.677, 14.760 -14.200 -53.677, 13.563 -15.900 -54.466, 13.563 -14.200 -54.466, 12.250 -15.900 -55.043, 12.250 -14.200 -55.043, 10.858 -15.900 -55.391, 10.858 -14.200 -55.391, 9.428 -15.900 -55.500, 9.428 -14.200 -55.500, 18.000 -15.900 -47.500, 10.009 -15.900 -50.967, 10.997 -15.900 -50.709, 8.988 -15.900 -50.967, 12.993 -15.900 -51.500, 11.888 -15.900 -50.209, 12.808 -14.200 -44.751, 8.000 -14.200 -50.708, 6.982 -14.200 -50.108, 12.135 -14.200 -43.991, 6.183 -14.200 -49.236, 13.267 -14.200 -45.656, 5.629 -14.200 -45.993, 5.500 -14.200 -47.000, 5.675 -14.200 -48.169, 13.484 -14.200 -46.648, 6.007 -14.200 -45.051, 13.445 -14.200 -47.662, 6.610 -14.200 -44.235, 13.151 -14.200 -48.634, 7.399 -14.200 -43.596, 12.622 -14.200 -49.500, 8.323 -14.200 -43.177, 11.888 -14.200 -50.209, 9.324 -14.200 -43.004, 10.997 -14.200 -50.709, 10.335 -14.200 -43.088, 10.009 -14.200 -50.967, 11.293 -14.200 -43.424, 8.988 -14.200 -50.967, 15.391 -15.900 -40.873, 15.627 -15.900 -41.109, 16.058 -15.900 -41.558, 17.472 -15.900 -44.052, 16.883 -15.900 -42.788, 18.000 -15.900 -45.723, 5.794 -14.200 -39.351, 4.373 -14.200 -40.220, 7.357 -14.200 -38.775, 13.799 -14.200 -39.667, 9.002 -14.200 -38.515, 9.760 -14.200 -35.823, 10.666 -14.200 -38.580, 12.286 -14.200 -38.970, 15.391 -14.200 -40.873, 15.627 -14.200 -41.109, 15.854 -14.200 -41.354, 10.550 -0.000 0.000, 3.215 -14.200 -52.723, 2.271 -14.200 -51.472, 1.573 -14.200 -50.068, 1.145 -14.200 -48.561, 1.000 -14.200 -47.000, 3.215 -14.200 -41.277, 2.271 -14.200 -42.528, 1.145 -14.200 -45.439, 1.573 -14.200 -43.932, 1.531 -14.200 -43.250, -2.156 -14.200 -43.250, -3.000 -14.200 -55.500, -3.000 -14.200 -31.000, -9.900 0.000 -31.000, 3.083 14.300 -52.574, 4.177 14.300 -53.627, 2.194 14.300 -51.344, 1.538 14.300 -49.976, 1.136 14.300 -48.512, 1.000 14.300 -47.000, 4.177 14.300 -40.373, 3.083 14.300 -41.426, 0.589 14.300 -43.250, 1.136 14.300 -45.488, 1.538 14.300 -44.024, 2.194 14.300 -42.656, 12.219 14.300 -38.947, 13.768 14.300 -39.649, 5.603 14.300 -39.446, 7.185 14.300 -38.821, 9.662 14.300 -35.823, 8.860 14.300 -38.524, 10.561 14.300 -38.566, 18.000 16.000 -47.500, 9.328 16.000 -51.997, 10.669 16.000 -51.861, 10.858 16.000 -55.391, 12.993 16.000 -51.500, 11.925 16.000 -51.372, 13.005 16.000 -50.566, 15.808 16.000 -52.697, 14.760 16.000 -53.677, 13.563 16.000 -54.466, 17.780 16.000 -48.920, 17.340 16.000 -50.284, 16.676 16.000 -51.556, 12.250 16.000 -55.043, 9.428 16.000 -55.500, 4.631 14.300 -45.865, 4.500 14.300 -47.000, 5.016 14.300 -44.789, 5.635 14.300 -43.828, 6.456 14.300 -43.033, 7.436 14.300 -42.446, 8.525 14.300 -42.096, 9.664 14.300 -42.003, 10.794 14.300 -42.170, 11.857 14.300 -42.590, 12.797 14.300 -43.241, 13.564 14.300 -44.088, 14.119 14.300 -45.087, 14.433 14.300 -46.186, 14.489 14.300 -47.327, 14.285 14.300 -48.451, 13.830 14.300 -49.500, 13.005 14.300 -50.566, 11.925 14.300 -51.372, 10.669 14.300 -51.861, 9.328 14.300 -51.997, 8.000 14.300 -51.770, 6.853 14.300 -51.242, 5.875 14.300 -50.443, 5.128 14.300 -49.425, 4.659 14.300 -48.253, 15.391 16.000 -40.873, 15.627 16.000 -41.109, 16.058 16.000 -41.558, 16.883 16.000 -42.788, 17.472 16.000 -44.052, 18.000 16.000 -45.723, 6.635 14.300 -55.002, 5.348 14.300 -54.417, 17.985 14.300 -47.500, 17.780 14.300 -48.920, 17.340 14.300 -50.284, 16.676 14.300 -51.556, 15.808 14.300 -52.697, 14.760 14.300 -53.677, 13.563 14.300 -54.466, 12.250 14.300 -55.043, 10.858 14.300 -55.391, 9.428 14.300 -55.500, 0.589 15.150 -55.500, 15.391 14.300 -40.873, 15.627 14.300 -41.109, 15.854 14.300 -41.354, -2.156 -14.250 -55.500, 17.847 -14.200 -45.394, 16.095 -14.200 -41.638, 16.016 -14.200 -41.542, 15.936 -14.200 -41.447, 18.000 -14.200 -47.000, 17.998 -14.200 -47.167, 15.000 -14.200 -47.500, 17.993 -14.200 -47.333, 17.932 -14.200 -45.926, 17.983 -14.200 -46.462, 15.000 -14.200 -49.500, 18.000 -14.200 -47.500, 12.993 -14.200 -51.500, 16.058 -14.200 -41.558, 16.883 -14.200 -42.788, 17.472 -14.200 -44.052, 18.000 -14.200 -45.723, 2.686 -14.200 -47.000, 2.589 14.300 -47.000, 16.095 14.300 -41.638, 17.847 14.300 -45.394, 16.016 14.300 -41.542, 15.936 14.300 -41.447, 15.000 14.300 -49.500, 15.000 14.300 -47.500, 17.998 14.300 -47.167, 18.000 14.300 -47.000, 17.993 14.300 -47.333, 17.983 14.300 -46.462, 17.932 14.300 -45.926, 18.000 14.300 -47.500, 12.993 14.300 -51.500, 16.058 14.300 -41.558, 16.883 14.300 -42.788, 17.472 14.300 -44.052, 18.000 14.300 -45.723 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 3, 0, 2, -1, 4, 3, 2, -1, 5, 4, 2, -1, 6, 5, 2, -1, 7, 6, 2, -1, 8, 7, 2, -1, 9, 8, 2, -1, 10, 9, 2, -1, 11, 10, 2, -1, 12, 11, 2, -1, 13, 12, 2, -1, 14, 13, 2, -1, 15, 14, 2, -1, 16, 15, 2, -1, 17, 16, 2, -1, 18, 17, 2, -1, 19, 18, 2, -1, 20, 19, 2, -1, 21, 20, 2, -1, 22, 21, 2, -1, 23, 22, 2, -1, 24, 23, 2, -1, 25, 24, 2, -1, 26, 25, 2, -1, 27, 26, 2, -1, 28, 27, 2, -1, 29, 28, 2, -1, 30, 29, 2, -1, 31, 30, 2, -1, 32, 31, 2, -1, 33, 32, 2, -1, 34, 33, 2, -1, 35, 34, 2, -1, 36, 35, 2, -1, 37, 36, 2, -1, 38, 37, 2, -1, 39, 38, 2, -1, 40, 39, 2, -1, 41, 40, 2, -1, 42, 41, 2, -1, 43, 42, 2, -1, 44, 43, 2, -1, 45, 44, 2, -1, 46, 45, 2, -1, 47, 46, 2, -1, 48, 47, 2, -1, 49, 48, 2, -1, 50, 49, 2, -1, 51, 50, 2, -1, 52, 51, 2, -1, 53, 52, 2, -1, 54, 53, 2, -1, 55, 54, 2, -1, 56, 55, 2, -1, 1, 56, 2, -1, 0, 57, 58, -1, 59, 0, 3, -1, 59, 57, 0, -1, 4, 59, 3, -1, 60, 4, 5, -1, 60, 59, 4, -1, 6, 60, 5, -1, 7, 60, 6, -1, 61, 60, 7, -1, 61, 7, 8, -1, 9, 61, 8, -1, 62, 61, 9, -1, 62, 9, 10, -1, 11, 62, 10, -1, 12, 62, 11, -1, 63, 62, 12, -1, 13, 63, 12, -1, 14, 63, 13, -1, 64, 14, 15, -1, 64, 63, 14, -1, 65, 64, 15, -1, 66, 65, 17, -1, 66, 64, 65, -1, 18, 66, 17, -1, 19, 66, 18, -1, 67, 66, 19, -1, 20, 67, 19, -1, 21, 67, 20, -1, 68, 21, 22, -1, 68, 67, 21, -1, 23, 68, 22, -1, 69, 23, 24, -1, 69, 68, 23, -1, 25, 69, 24, -1, 26, 69, 25, -1, 70, 69, 26, -1, 27, 70, 26, -1, 28, 70, 27, -1, 71, 28, 29, -1, 71, 70, 28, -1, 30, 71, 29, -1, 72, 30, 31, -1, 72, 71, 30, -1, 32, 72, 31, -1, 73, 32, 33, -1, 73, 72, 32, -1, 34, 73, 33, -1, 35, 73, 34, -1, 74, 35, 36, -1, 74, 73, 35, -1, 37, 74, 36, -1, 75, 37, 38, -1, 75, 74, 37, -1, 39, 75, 38, -1, 76, 39, 40, -1, 76, 75, 39, -1, 41, 76, 40, -1, 42, 76, 41, -1, 77, 76, 42, -1, 77, 42, 43, -1, 44, 77, 43, -1, 78, 77, 44, -1, 78, 44, 79, -1, 46, 78, 79, -1, 80, 78, 46, -1, 80, 46, 47, -1, 48, 80, 47, -1, 49, 80, 48, -1, 81, 80, 49, -1, 81, 49, 50, -1, 51, 81, 50, -1, 82, 81, 51, -1, 82, 51, 52, -1, 53, 82, 52, -1, 83, 82, 53, -1, 83, 53, 54, -1, 55, 83, 54, -1, 56, 83, 55, -1, 57, 83, 56, -1, 57, 56, 58, -1, 68, 69, 84, -1, 67, 68, 84, -1, 66, 67, 84, -1, 83, 85, 82, -1, 64, 66, 84, -1, 72, 73, 86, -1, 87, 83, 57, -1, 87, 57, 59, -1, 87, 59, 60, -1, 87, 60, 61, -1, 87, 61, 62, -1, 87, 62, 63, -1, 87, 63, 64, -1, 87, 64, 84, -1, 87, 85, 83, -1, 88, 77, 78, -1, 88, 78, 80, -1, 88, 80, 81, -1, 88, 81, 82, -1, 88, 73, 74, -1, 88, 74, 75, -1, 88, 75, 76, -1, 88, 76, 77, -1, 88, 89, 86, -1, 88, 90, 89, -1, 88, 91, 90, -1, 84, 69, 70, -1, 88, 92, 91, -1, 84, 70, 71, -1, 88, 85, 92, -1, 84, 71, 72, -1, 88, 86, 73, -1, 88, 82, 85, -1, 84, 72, 86, -1, 93, 94, 86, -1, 93, 86, 95, -1, 95, 86, 89, -1, 96, 97, 98, -1, 97, 84, 98, -1, 99, 96, 98, -1, 84, 86, 98, -1, 100, 99, 98, -1, 86, 94, 98, -1, 101, 100, 98, -1, 94, 102, 98, -1, 102, 101, 98, -1, 103, 91, 92, -1, 104, 105, 91, -1, 104, 91, 103, -1, 106, 107, 105, -1, 106, 105, 104, -1, 108, 109, 107, -1, 108, 107, 106, -1, 110, 109, 108, -1, 111, 112, 110, -1, 111, 110, 108, -1, 113, 90, 112, -1, 113, 112, 111, -1, 114, 90, 113, -1, 89, 90, 114, -1, 115, 116, 117, -1, 115, 117, 118, -1, 119, 90, 91, -1, 119, 109, 110, -1, 119, 110, 112, -1, 119, 112, 90, -1, 119, 91, 105, -1, 119, 105, 107, -1, 119, 107, 109, -1, 120, 121, 122, -1, 120, 87, 121, -1, 120, 122, 123, -1, 120, 85, 87, -1, 120, 123, 124, -1, 120, 125, 85, -1, 120, 124, 126, -1, 120, 127, 125, -1, 120, 126, 127, -1, 125, 128, 85, -1, 128, 129, 85, -1, 129, 92, 85, -1, 130, 131, 132, -1, 133, 130, 132, -1, 134, 108, 135, -1, 136, 133, 137, -1, 136, 130, 133, -1, 111, 108, 134, -1, 138, 111, 134, -1, 113, 111, 138, -1, 139, 113, 138, -1, 114, 139, 140, -1, 114, 113, 139, -1, 141, 114, 140, -1, 89, 114, 141, -1, 95, 89, 141, -1, 142, 93, 95, -1, 142, 95, 141, -1, 94, 93, 142, -1, 143, 94, 142, -1, 144, 94, 143, -1, 145, 144, 143, -1, 146, 144, 145, -1, 147, 146, 145, -1, 148, 146, 147, -1, 131, 146, 148, -1, 132, 131, 148, -1, 96, 149, 150, -1, 149, 96, 151, -1, 96, 150, 152, -1, 151, 96, 153, -1, 96, 152, 154, -1, 153, 96, 155, -1, 96, 154, 156, -1, 96, 156, 157, -1, 96, 157, 158, -1, 159, 122, 160, -1, 161, 162, 97, -1, 163, 161, 97, -1, 164, 163, 97, -1, 165, 164, 97, -1, 166, 165, 97, -1, 167, 166, 97, -1, 158, 167, 97, -1, 96, 158, 97, -1, 160, 122, 168, -1, 97, 162, 169, -1, 168, 122, 170, -1, 97, 169, 171, -1, 170, 122, 172, -1, 97, 171, 173, -1, 172, 122, 174, -1, 97, 173, 175, -1, 174, 122, 176, -1, 97, 175, 177, -1, 97, 177, 178, -1, 97, 178, 179, -1, 97, 179, 180, -1, 97, 180, 181, -1, 182, 183, 121, -1, 184, 182, 121, -1, 185, 184, 121, -1, 186, 185, 121, -1, 187, 186, 121, -1, 188, 187, 121, -1, 189, 188, 121, -1, 190, 189, 121, -1, 191, 190, 121, -1, 192, 191, 121, -1, 193, 192, 121, -1, 194, 193, 121, -1, 195, 194, 121, -1, 181, 195, 121, -1, 97, 181, 121, -1, 183, 122, 121, -1, 196, 122, 183, -1, 197, 122, 196, -1, 198, 122, 197, -1, 176, 122, 198, -1, 96, 122, 199, -1, 122, 159, 199, -1, 200, 155, 199, -1, 201, 200, 199, -1, 202, 201, 199, -1, 203, 202, 199, -1, 204, 203, 199, -1, 205, 204, 199, -1, 206, 205, 199, -1, 159, 206, 199, -1, 155, 96, 199, -1, 96, 99, 207, -1, 122, 208, 123, -1, 207, 208, 209, -1, 122, 96, 209, -1, 96, 207, 209, -1, 208, 122, 209, -1, 210, 99, 100, -1, 211, 210, 100, -1, 212, 210, 211, -1, 213, 210, 212, -1, 214, 215, 101, -1, 216, 213, 212, -1, 216, 212, 211, -1, 216, 211, 100, -1, 216, 215, 213, -1, 216, 100, 101, -1, 216, 101, 215, -1, 130, 217, 218, -1, 130, 219, 217, -1, 131, 218, 220, -1, 131, 130, 218, -1, 144, 221, 102, -1, 144, 146, 221, -1, 94, 144, 102, -1, 136, 219, 130, -1, 222, 220, 221, -1, 222, 146, 131, -1, 222, 131, 220, -1, 222, 221, 146, -1, 223, 214, 101, -1, 218, 217, 223, -1, 218, 223, 101, -1, 224, 102, 221, -1, 224, 221, 220, -1, 224, 220, 218, -1, 224, 101, 102, -1, 224, 218, 101, -1, 225, 104, 226, -1, 106, 104, 225, -1, 227, 228, 229, -1, 230, 106, 225, -1, 108, 230, 135, -1, 231, 228, 227, -1, 108, 106, 230, -1, 232, 231, 227, -1, 233, 231, 232, -1, 234, 233, 232, -1, 235, 234, 236, -1, 235, 233, 234, -1, 237, 235, 236, -1, 238, 235, 237, -1, 239, 238, 237, -1, 240, 241, 239, -1, 240, 239, 237, -1, 125, 241, 240, -1, 128, 125, 240, -1, 129, 128, 240, -1, 242, 129, 240, -1, 92, 129, 242, -1, 243, 92, 242, -1, 103, 92, 243, -1, 226, 103, 243, -1, 104, 103, 226, -1, 124, 123, 244, -1, 245, 124, 244, -1, 246, 245, 244, -1, 247, 246, 244, -1, 126, 248, 249, -1, 126, 124, 248, -1, 250, 251, 248, -1, 250, 247, 251, -1, 250, 124, 245, -1, 250, 245, 246, -1, 250, 246, 247, -1, 250, 248, 124, -1, 252, 238, 239, -1, 241, 252, 239, -1, 125, 252, 241, -1, 253, 252, 254, -1, 254, 252, 255, -1, 255, 252, 127, -1, 127, 252, 125, -1, 256, 126, 249, -1, 256, 127, 126, -1, 256, 253, 254, -1, 256, 254, 255, -1, 256, 255, 127, -1, 256, 249, 253, -1, 257, 258, 259, -1, 257, 260, 258, -1, 261, 262, 263, -1, 257, 264, 260, -1, 257, 265, 264, -1, 266, 259, 267, -1, 266, 267, 268, -1, 266, 268, 269, -1, 270, 271, 272, -1, 266, 257, 259, -1, 270, 261, 271, -1, 273, 274, 275, -1, 273, 276, 274, -1, 277, 269, 278, -1, 277, 266, 269, -1, 279, 275, 280, -1, 279, 273, 275, -1, 281, 280, 282, -1, 283, 284, 285, -1, 283, 272, 284, -1, 283, 270, 272, -1, 286, 281, 287, -1, 286, 279, 280, -1, 286, 280, 281, -1, 288, 289, 290, -1, 288, 287, 289, -1, 288, 286, 287, -1, 291, 292, 293, -1, 291, 290, 292, -1, 291, 288, 290, -1, 265, 294, 264, -1, 265, 285, 294, -1, 265, 283, 285, -1, 262, 293, 263, -1, 262, 291, 293, -1, 261, 295, 271, -1, 261, 263, 295, -1, 296, 297, 298, -1, 299, 300, 301, -1, 299, 301, 302, -1, 299, 302, 303, -1, 299, 303, 304, -1, 299, 304, 305, -1, 299, 305, 306, -1, 278, 296, 307, -1, 278, 307, 308, -1, 278, 308, 309, -1, 277, 299, 310, -1, 278, 297, 296, -1, 277, 311, 312, -1, 277, 312, 313, -1, 277, 313, 314, -1, 277, 314, 315, -1, 277, 315, 300, -1, 277, 300, 299, -1, 316, 278, 309, -1, 317, 277, 310, -1, 277, 278, 318, -1, 277, 318, 319, -1, 277, 319, 311, -1, 320, 299, 306, -1, 321, 278, 316, -1, 322, 278, 321, -1, 318, 278, 322, -1, 323, 183, 182, -1, 323, 324, 183, -1, 184, 323, 182, -1, 325, 323, 184, -1, 185, 325, 184, -1, 326, 325, 185, -1, 186, 326, 185, -1, 327, 326, 186, -1, 187, 327, 186, -1, 328, 327, 187, -1, 188, 328, 187, -1, 329, 328, 188, -1, 189, 329, 188, -1, 330, 329, 189, -1, 190, 330, 189, -1, 331, 330, 190, -1, 191, 331, 190, -1, 332, 331, 191, -1, 192, 332, 191, -1, 333, 332, 192, -1, 193, 333, 192, -1, 334, 333, 193, -1, 194, 334, 193, -1, 335, 334, 194, -1, 195, 335, 194, -1, 336, 335, 195, -1, 337, 195, 181, -1, 337, 336, 195, -1, 180, 337, 181, -1, 338, 337, 180, -1, 339, 180, 179, -1, 339, 338, 180, -1, 178, 339, 179, -1, 340, 339, 178, -1, 341, 178, 177, -1, 341, 340, 178, -1, 175, 341, 177, -1, 342, 341, 175, -1, 343, 175, 173, -1, 343, 342, 175, -1, 171, 343, 173, -1, 344, 343, 171, -1, 345, 344, 171, -1, 345, 171, 169, -1, 162, 345, 169, -1, 346, 345, 162, -1, 161, 346, 162, -1, 347, 346, 161, -1, 163, 347, 161, -1, 348, 347, 163, -1, 164, 348, 163, -1, 349, 348, 164, -1, 350, 349, 164, -1, 350, 164, 165, -1, 166, 350, 165, -1, 351, 350, 166, -1, 167, 351, 166, -1, 352, 351, 167, -1, 158, 352, 167, -1, 353, 352, 158, -1, 157, 353, 158, -1, 354, 353, 157, -1, 355, 157, 156, -1, 355, 354, 157, -1, 154, 355, 156, -1, 356, 355, 154, -1, 152, 356, 154, -1, 357, 356, 152, -1, 150, 357, 152, -1, 358, 357, 150, -1, 359, 150, 149, -1, 359, 358, 150, -1, 151, 359, 149, -1, 360, 359, 151, -1, 153, 360, 151, -1, 361, 360, 153, -1, 362, 153, 155, -1, 362, 361, 153, -1, 200, 362, 155, -1, 363, 362, 200, -1, 201, 363, 200, -1, 364, 363, 201, -1, 202, 364, 201, -1, 365, 364, 202, -1, 366, 202, 203, -1, 366, 365, 202, -1, 204, 366, 203, -1, 367, 366, 204, -1, 205, 367, 204, -1, 368, 367, 205, -1, 206, 368, 205, -1, 369, 368, 206, -1, 159, 369, 206, -1, 370, 369, 159, -1, 160, 370, 159, -1, 371, 370, 160, -1, 168, 371, 160, -1, 372, 371, 168, -1, 170, 372, 168, -1, 373, 372, 170, -1, 172, 373, 170, -1, 374, 373, 172, -1, 174, 374, 172, -1, 375, 374, 174, -1, 176, 375, 174, -1, 376, 375, 176, -1, 377, 376, 176, -1, 377, 176, 198, -1, 197, 377, 198, -1, 378, 377, 197, -1, 196, 378, 197, -1, 379, 378, 196, -1, 183, 379, 196, -1, 324, 379, 183, -1, 99, 380, 207, -1, 99, 210, 381, -1, 382, 380, 381, -1, 210, 382, 381, -1, 380, 99, 381, -1, 380, 383, 208, -1, 207, 380, 208, -1, 208, 383, 123, -1, 244, 123, 384, -1, 385, 244, 384, -1, 383, 385, 384, -1, 123, 383, 384, -1, 386, 387, 388, -1, 386, 389, 387, -1, 386, 390, 389, -1, 386, 391, 390, -1, 386, 392, 391, -1, 386, 388, 392, -1, 393, 394, 395, -1, 393, 396, 394, -1, 393, 397, 396, -1, 398, 391, 397, -1, 398, 387, 389, -1, 398, 389, 390, -1, 398, 390, 391, -1, 398, 393, 387, -1, 398, 397, 393, -1, 214, 394, 396, -1, 214, 396, 397, -1, 395, 394, 214, -1, 393, 399, 217, -1, 393, 400, 399, -1, 393, 395, 400, -1, 219, 393, 217, -1, 219, 317, 393, -1, 387, 393, 401, -1, 401, 393, 402, -1, 402, 393, 317, -1, 400, 395, 214, -1, 217, 399, 223, -1, 403, 214, 223, -1, 403, 399, 400, -1, 403, 400, 214, -1, 403, 223, 399, -1, 404, 405, 406, -1, 407, 408, 409, -1, 410, 407, 409, -1, 411, 410, 409, -1, 412, 411, 409, -1, 413, 412, 409, -1, 414, 415, 416, -1, 417, 414, 416, -1, 408, 417, 416, -1, 418, 409, 416, -1, 419, 420, 421, -1, 409, 408, 416, -1, 422, 406, 421, -1, 420, 422, 421, -1, 423, 424, 229, -1, 415, 423, 229, -1, 406, 405, 421, -1, 416, 415, 229, -1, 419, 421, 425, -1, 426, 416, 228, -1, 416, 229, 228, -1, 421, 424, 427, -1, 229, 424, 421, -1, 413, 409, 428, -1, 425, 421, 429, -1, 429, 421, 427, -1, 430, 431, 432, -1, 430, 432, 433, -1, 434, 430, 433, -1, 434, 433, 435, -1, 436, 437, 438, -1, 436, 439, 437, -1, 436, 440, 439, -1, 441, 434, 435, -1, 441, 435, 274, -1, 442, 443, 444, -1, 276, 441, 274, -1, 445, 438, 446, -1, 445, 436, 438, -1, 229, 421, 447, -1, 229, 447, 443, -1, 448, 449, 450, -1, 448, 446, 449, -1, 229, 443, 442, -1, 448, 445, 446, -1, 451, 452, 453, -1, 451, 442, 452, -1, 451, 453, 454, -1, 451, 229, 442, -1, 455, 456, 457, -1, 455, 454, 456, -1, 455, 451, 454, -1, 458, 450, 459, -1, 458, 448, 450, -1, 433, 432, 460, -1, 431, 461, 432, -1, 440, 462, 439, -1, 431, 463, 461, -1, 440, 457, 462, -1, 431, 459, 463, -1, 440, 455, 457, -1, 431, 458, 459, -1, 464, 426, 465, -1, 426, 228, 465, -1, 233, 235, 466, -1, 235, 238, 466, -1, 228, 231, 467, -1, 231, 233, 467, -1, 466, 465, 467, -1, 465, 228, 467, -1, 233, 466, 467, -1, 468, 469, 464, -1, 470, 247, 244, -1, 470, 471, 247, -1, 470, 469, 471, -1, 470, 472, 464, -1, 470, 244, 472, -1, 470, 464, 469, -1, 248, 473, 474, -1, 473, 475, 474, -1, 249, 248, 474, -1, 475, 252, 474, -1, 253, 249, 474, -1, 252, 253, 474, -1, 248, 251, 476, -1, 473, 248, 476, -1, 251, 468, 476, -1, 477, 473, 476, -1, 468, 464, 476, -1, 465, 477, 476, -1, 464, 465, 476, -1, 251, 471, 469, -1, 251, 469, 468, -1, 251, 247, 471, -1, 478, 238, 252, -1, 478, 252, 475, -1, 478, 466, 238, -1, 478, 475, 479, -1, 478, 479, 466, -1, 480, 258, 260, -1, 481, 480, 260, -1, 482, 483, 267, -1, 482, 484, 483, -1, 482, 480, 484, -1, 482, 267, 259, -1, 482, 259, 258, -1, 482, 258, 480, -1, 485, 486, 487, -1, 486, 488, 487, -1, 269, 268, 487, -1, 483, 485, 267, -1, 487, 268, 267, -1, 485, 487, 267, -1, 269, 487, 278, -1, 271, 489, 272, -1, 272, 489, 490, -1, 282, 491, 492, -1, 272, 490, 284, -1, 282, 492, 281, -1, 284, 490, 493, -1, 281, 492, 494, -1, 284, 493, 285, -1, 281, 494, 495, -1, 285, 493, 496, -1, 281, 495, 287, -1, 285, 496, 294, -1, 287, 495, 497, -1, 294, 496, 498, -1, 294, 498, 264, -1, 287, 497, 289, -1, 264, 498, 499, -1, 289, 497, 500, -1, 264, 499, 260, -1, 260, 499, 481, -1, 289, 500, 290, -1, 290, 500, 501, -1, 290, 501, 292, -1, 292, 501, 502, -1, 292, 502, 293, -1, 293, 502, 503, -1, 293, 503, 263, -1, 263, 503, 504, -1, 263, 504, 295, -1, 295, 504, 505, -1, 295, 505, 271, -1, 271, 505, 489, -1, 433, 460, 506, -1, 506, 507, 435, -1, 433, 506, 435, -1, 435, 507, 508, -1, 435, 508, 274, -1, 509, 510, 275, -1, 510, 491, 275, -1, 274, 509, 275, -1, 275, 491, 280, -1, 280, 491, 282, -1, 508, 511, 512, -1, 511, 509, 512, -1, 509, 274, 512, -1, 274, 508, 512, -1, 513, 297, 514, -1, 515, 297, 278, -1, 515, 516, 517, -1, 516, 514, 517, -1, 297, 515, 517, -1, 514, 297, 517, -1, 298, 297, 518, -1, 297, 513, 518, -1, 519, 317, 310, -1, 519, 402, 317, -1, 519, 401, 402, -1, 519, 387, 401, -1, 519, 388, 387, -1, 519, 310, 388, -1, 520, 521, 522, -1, 520, 523, 521, -1, 520, 524, 523, -1, 520, 525, 526, -1, 520, 527, 525, -1, 520, 522, 527, -1, 528, 529, 520, -1, 528, 530, 529, -1, 531, 528, 520, -1, 532, 531, 520, -1, 533, 520, 526, -1, 533, 532, 520, -1, 534, 296, 298, -1, 534, 307, 296, -1, 535, 533, 526, -1, 536, 533, 535, -1, 537, 536, 535, -1, 538, 536, 537, -1, 539, 536, 538, -1, 540, 308, 307, -1, 540, 307, 534, -1, 541, 536, 539, -1, 320, 541, 539, -1, 309, 308, 540, -1, 542, 309, 540, -1, 306, 541, 320, -1, 306, 543, 541, -1, 316, 309, 542, -1, 316, 542, 544, -1, 305, 545, 543, -1, 305, 543, 306, -1, 321, 316, 544, -1, 304, 545, 305, -1, 322, 544, 546, -1, 322, 321, 544, -1, 303, 545, 304, -1, 303, 547, 545, -1, 318, 322, 546, -1, 302, 548, 547, -1, 302, 547, 303, -1, 319, 546, 549, -1, 319, 318, 546, -1, 301, 548, 302, -1, 311, 549, 550, -1, 311, 319, 549, -1, 300, 551, 548, -1, 300, 548, 301, -1, 312, 311, 550, -1, 315, 551, 300, -1, 313, 550, 552, -1, 313, 312, 550, -1, 314, 552, 551, -1, 314, 313, 552, -1, 314, 551, 315, -1, 310, 553, 388, -1, 310, 299, 554, -1, 299, 555, 554, -1, 555, 556, 554, -1, 556, 553, 554, -1, 553, 310, 554, -1, 557, 299, 320, -1, 557, 555, 299, -1, 558, 359, 360, -1, 559, 359, 558, -1, 559, 358, 359, -1, 560, 558, 360, -1, 560, 360, 361, -1, 561, 358, 559, -1, 561, 356, 357, -1, 561, 357, 358, -1, 562, 356, 561, -1, 562, 355, 356, -1, 563, 560, 361, -1, 563, 361, 362, -1, 564, 563, 362, -1, 564, 362, 363, -1, 565, 355, 562, -1, 565, 354, 355, -1, 566, 564, 363, -1, 566, 363, 364, -1, 567, 354, 565, -1, 567, 353, 354, -1, 568, 566, 364, -1, 568, 364, 365, -1, 569, 352, 353, -1, 569, 353, 567, -1, 570, 351, 352, -1, 570, 352, 569, -1, 571, 568, 365, -1, 571, 365, 366, -1, 572, 350, 351, -1, 572, 351, 570, -1, 573, 571, 366, -1, 573, 366, 367, -1, 574, 573, 367, -1, 574, 367, 368, -1, 575, 350, 572, -1, 575, 576, 350, -1, 577, 574, 368, -1, 577, 368, 369, -1, 577, 369, 370, -1, 578, 576, 575, -1, 578, 579, 576, -1, 580, 577, 370, -1, 580, 370, 581, -1, 582, 579, 578, -1, 582, 347, 579, -1, 583, 581, 372, -1, 583, 580, 581, -1, 584, 347, 582, -1, 584, 345, 346, -1, 584, 346, 347, -1, 585, 583, 372, -1, 586, 345, 584, -1, 373, 585, 372, -1, 373, 587, 585, -1, 344, 345, 586, -1, 344, 586, 588, -1, 374, 587, 373, -1, 374, 589, 587, -1, 590, 588, 591, -1, 590, 344, 588, -1, 375, 589, 374, -1, 375, 592, 589, -1, 593, 591, 594, -1, 593, 590, 591, -1, 376, 592, 375, -1, 376, 595, 592, -1, 596, 594, 597, -1, 596, 593, 594, -1, 377, 595, 376, -1, 377, 598, 595, -1, 599, 597, 600, -1, 599, 596, 597, -1, 378, 601, 598, -1, 378, 598, 377, -1, 602, 600, 603, -1, 602, 599, 600, -1, 379, 604, 601, -1, 379, 601, 378, -1, 605, 603, 606, -1, 605, 602, 603, -1, 324, 604, 379, -1, 607, 606, 608, -1, 607, 605, 606, -1, 323, 609, 604, -1, 323, 604, 324, -1, 610, 608, 611, -1, 610, 607, 608, -1, 325, 612, 609, -1, 325, 609, 323, -1, 613, 610, 611, -1, 613, 611, 614, -1, 615, 616, 612, -1, 615, 612, 325, -1, 617, 613, 614, -1, 618, 619, 616, -1, 618, 616, 615, -1, 620, 617, 614, -1, 620, 614, 621, -1, 622, 619, 618, -1, 622, 623, 619, -1, 624, 621, 625, -1, 624, 620, 621, -1, 626, 627, 623, -1, 626, 623, 622, -1, 628, 625, 629, -1, 628, 624, 625, -1, 630, 629, 627, -1, 630, 627, 626, -1, 630, 628, 629, -1, 631, 632, 556, -1, 631, 556, 633, -1, 633, 556, 634, -1, 388, 553, 392, -1, 553, 556, 392, -1, 633, 634, 635, -1, 636, 632, 637, -1, 631, 633, 637, -1, 632, 631, 637, -1, 635, 638, 637, -1, 638, 639, 637, -1, 639, 640, 637, -1, 641, 636, 637, -1, 642, 641, 637, -1, 640, 642, 637, -1, 633, 635, 637, -1, 643, 644, 645, -1, 645, 644, 646, -1, 641, 643, 636, -1, 643, 641, 642, -1, 643, 642, 640, -1, 647, 648, 649, -1, 650, 651, 652, -1, 651, 653, 652, -1, 653, 654, 652, -1, 655, 656, 652, -1, 656, 650, 652, -1, 647, 649, 652, -1, 652, 649, 657, -1, 658, 655, 659, -1, 655, 652, 659, -1, 652, 657, 659, -1, 660, 661, 662, -1, 658, 659, 663, -1, 661, 664, 665, -1, 662, 661, 665, -1, 658, 663, 666, -1, 666, 663, 667, -1, 666, 667, 668, -1, 668, 667, 669, -1, 669, 667, 670, -1, 665, 664, 671, -1, 665, 671, 672, -1, 670, 667, 673, -1, 670, 673, 674, -1, 672, 671, 675, -1, 676, 672, 675, -1, 673, 677, 678, -1, 674, 673, 678, -1, 676, 675, 679, -1, 680, 676, 679, -1, 677, 681, 682, -1, 678, 677, 682, -1, 683, 680, 684, -1, 680, 679, 684, -1, 681, 685, 686, -1, 682, 681, 686, -1, 683, 684, 687, -1, 685, 688, 689, -1, 686, 685, 689, -1, 689, 688, 690, -1, 691, 683, 692, -1, 683, 687, 692, -1, 688, 693, 694, -1, 690, 688, 694, -1, 695, 691, 696, -1, 691, 692, 696, -1, 693, 697, 698, -1, 694, 693, 698, -1, 699, 695, 700, -1, 695, 696, 700, -1, 697, 699, 701, -1, 698, 697, 701, -1, 699, 700, 701, -1, 702, 703, 660, -1, 703, 704, 660, -1, 704, 703, 705, -1, 704, 705, 706, -1, 704, 706, 707, -1, 704, 707, 421, -1, 674, 708, 709, -1, 674, 710, 708, -1, 643, 708, 644, -1, 708, 710, 644, -1, 464, 416, 426, -1, 418, 416, 711, -1, 472, 418, 711, -1, 464, 472, 711, -1, 416, 464, 711, -1, 442, 712, 713, -1, 442, 444, 712, -1, 714, 715, 453, -1, 714, 716, 715, -1, 714, 713, 716, -1, 714, 453, 452, -1, 714, 452, 442, -1, 714, 442, 713, -1, 447, 421, 717, -1, 443, 447, 717, -1, 444, 443, 717, -1, 421, 718, 717, -1, 718, 719, 717, -1, 719, 720, 717, -1, 720, 712, 717, -1, 712, 444, 717, -1, 721, 439, 722, -1, 722, 439, 462, -1, 723, 506, 460, -1, 722, 462, 724, -1, 724, 462, 457, -1, 724, 457, 725, -1, 725, 457, 456, -1, 726, 723, 432, -1, 727, 726, 432, -1, 723, 460, 432, -1, 725, 456, 728, -1, 728, 456, 454, -1, 729, 727, 461, -1, 728, 454, 715, -1, 727, 432, 461, -1, 715, 454, 453, -1, 729, 461, 730, -1, 730, 461, 463, -1, 730, 463, 731, -1, 731, 463, 459, -1, 732, 731, 450, -1, 731, 459, 450, -1, 732, 450, 733, -1, 733, 450, 449, -1, 733, 449, 734, -1, 734, 449, 446, -1, 734, 446, 735, -1, 735, 446, 438, -1, 735, 438, 736, -1, 736, 438, 437, -1, 736, 437, 721, -1, 721, 437, 439, -1, 737, 738, 739, -1, 737, 740, 738, -1, 739, 741, 473, -1, 739, 473, 737, -1, 489, 505, 742, -1, 505, 743, 742, -1, 499, 498, 744, -1, 498, 496, 744, -1, 491, 510, 492, -1, 496, 745, 744, -1, 490, 489, 746, -1, 492, 510, 494, -1, 489, 742, 746, -1, 509, 511, 747, -1, 484, 480, 748, -1, 480, 481, 748, -1, 481, 499, 748, -1, 499, 744, 748, -1, 484, 748, 749, -1, 485, 483, 749, -1, 486, 485, 749, -1, 488, 486, 749, -1, 483, 484, 749, -1, 497, 495, 750, -1, 495, 494, 750, -1, 510, 509, 750, -1, 509, 747, 750, -1, 494, 510, 750, -1, 501, 500, 751, -1, 496, 493, 745, -1, 500, 497, 751, -1, 493, 490, 745, -1, 497, 750, 751, -1, 490, 746, 745, -1, 502, 501, 752, -1, 501, 751, 752, -1, 503, 502, 753, -1, 502, 752, 753, -1, 505, 504, 743, -1, 504, 503, 743, -1, 503, 753, 743, -1, 514, 516, 754, -1, 755, 756, 278, -1, 756, 754, 278, -1, 488, 755, 487, -1, 755, 278, 487, -1, 515, 278, 757, -1, 516, 515, 757, -1, 754, 516, 757, -1, 278, 754, 757, -1, 506, 723, 507, -1, 723, 726, 507, -1, 736, 721, 758, -1, 721, 759, 758, -1, 734, 735, 760, -1, 735, 736, 760, -1, 727, 729, 761, -1, 729, 730, 761, -1, 736, 758, 760, -1, 730, 762, 761, -1, 727, 761, 763, -1, 507, 726, 763, -1, 508, 507, 763, -1, 726, 727, 763, -1, 733, 734, 764, -1, 734, 760, 764, -1, 508, 763, 747, -1, 713, 712, 765, -1, 719, 718, 765, -1, 511, 508, 747, -1, 720, 719, 765, -1, 712, 720, 765, -1, 732, 733, 766, -1, 733, 764, 766, -1, 728, 715, 767, -1, 715, 716, 767, -1, 716, 713, 767, -1, 713, 765, 767, -1, 724, 725, 768, -1, 725, 728, 768, -1, 728, 767, 768, -1, 721, 722, 759, -1, 722, 724, 759, -1, 730, 731, 762, -1, 724, 768, 759, -1, 731, 732, 762, -1, 732, 766, 762, -1, 518, 513, 769, -1, 770, 769, 514, -1, 769, 513, 514, -1, 770, 514, 771, -1, 772, 773, 524, -1, 772, 524, 774, -1, 775, 774, 776, -1, 775, 772, 774, -1, 777, 776, 778, -1, 777, 775, 776, -1, 779, 778, 780, -1, 779, 777, 778, -1, 781, 779, 780, -1, 782, 779, 781, -1, 783, 782, 781, -1, 784, 782, 783, -1, 785, 784, 783, -1, 786, 784, 785, -1, 787, 786, 785, -1, 788, 786, 787, -1, 789, 788, 787, -1, 790, 788, 789, -1, 298, 790, 789, -1, 518, 790, 298, -1, 523, 524, 791, -1, 521, 523, 791, -1, 522, 521, 791, -1, 787, 785, 792, -1, 789, 787, 792, -1, 785, 793, 792, -1, 298, 789, 794, -1, 789, 792, 794, -1, 298, 794, 534, -1, 529, 530, 795, -1, 530, 796, 795, -1, 796, 793, 795, -1, 781, 780, 795, -1, 783, 781, 795, -1, 785, 783, 795, -1, 780, 529, 795, -1, 793, 785, 795, -1, 776, 774, 529, -1, 778, 776, 529, -1, 780, 778, 529, -1, 774, 524, 520, -1, 529, 774, 520, -1, 536, 541, 797, -1, 540, 798, 799, -1, 541, 800, 797, -1, 542, 540, 801, -1, 540, 799, 801, -1, 533, 536, 802, -1, 549, 546, 803, -1, 536, 797, 802, -1, 546, 804, 803, -1, 544, 542, 805, -1, 542, 801, 805, -1, 549, 803, 550, -1, 532, 533, 806, -1, 533, 802, 806, -1, 550, 803, 807, -1, 546, 544, 804, -1, 544, 805, 804, -1, 532, 806, 808, -1, 531, 532, 808, -1, 552, 550, 809, -1, 550, 807, 809, -1, 531, 808, 810, -1, 528, 531, 810, -1, 551, 552, 811, -1, 552, 809, 811, -1, 528, 810, 812, -1, 530, 528, 812, -1, 548, 551, 813, -1, 551, 811, 813, -1, 796, 530, 814, -1, 530, 812, 814, -1, 547, 548, 815, -1, 548, 813, 815, -1, 793, 796, 816, -1, 796, 814, 816, -1, 545, 547, 817, -1, 792, 793, 818, -1, 547, 815, 817, -1, 793, 816, 818, -1, 543, 545, 819, -1, 794, 792, 820, -1, 792, 818, 820, -1, 545, 817, 819, -1, 541, 543, 800, -1, 534, 794, 798, -1, 794, 820, 798, -1, 543, 819, 800, -1, 534, 798, 540, -1, 821, 320, 539, -1, 822, 821, 539, -1, 537, 535, 823, -1, 538, 537, 823, -1, 539, 538, 823, -1, 824, 535, 526, -1, 825, 535, 824, -1, 826, 525, 527, -1, 826, 527, 522, -1, 526, 525, 826, -1, 556, 555, 634, -1, 827, 828, 634, -1, 829, 827, 634, -1, 555, 829, 634, -1, 830, 555, 557, -1, 831, 829, 832, -1, 833, 831, 832, -1, 834, 833, 832, -1, 830, 834, 832, -1, 829, 555, 832, -1, 555, 830, 832, -1, 835, 320, 821, -1, 835, 557, 320, -1, 836, 821, 822, -1, 836, 835, 821, -1, 837, 822, 539, -1, 837, 836, 822, -1, 838, 604, 609, -1, 838, 609, 612, -1, 838, 612, 616, -1, 838, 616, 619, -1, 838, 619, 623, -1, 838, 623, 627, -1, 838, 627, 629, -1, 838, 629, 625, -1, 838, 625, 621, -1, 838, 621, 614, -1, 838, 614, 611, -1, 838, 611, 608, -1, 838, 608, 606, -1, 838, 606, 603, -1, 838, 603, 600, -1, 838, 600, 597, -1, 838, 597, 594, -1, 838, 594, 591, -1, 838, 591, 588, -1, 838, 588, 586, -1, 838, 586, 584, -1, 838, 584, 582, -1, 838, 582, 578, -1, 838, 578, 575, -1, 838, 575, 572, -1, 838, 572, 570, -1, 838, 570, 569, -1, 838, 569, 567, -1, 838, 567, 565, -1, 838, 565, 562, -1, 838, 562, 561, -1, 838, 561, 559, -1, 838, 559, 558, -1, 838, 558, 560, -1, 838, 560, 563, -1, 838, 563, 564, -1, 838, 564, 566, -1, 838, 566, 568, -1, 838, 568, 571, -1, 838, 571, 573, -1, 838, 573, 574, -1, 838, 574, 577, -1, 838, 577, 580, -1, 838, 580, 583, -1, 838, 583, 585, -1, 838, 585, 587, -1, 838, 587, 589, -1, 838, 589, 592, -1, 838, 592, 595, -1, 838, 595, 598, -1, 838, 598, 601, -1, 838, 601, 604, -1, 771, 514, 839, -1, 839, 514, 754, -1, 839, 754, 840, -1, 840, 754, 841, -1, 841, 754, 842, -1, 842, 754, 843, -1, 634, 828, 635, -1, 844, 845, 635, -1, 828, 844, 635, -1, 754, 846, 843, -1, 635, 846, 754, -1, 847, 846, 848, -1, 845, 847, 848, -1, 846, 635, 848, -1, 635, 845, 848, -1, 635, 754, 849, -1, 754, 850, 849, -1, 638, 635, 849, -1, 850, 851, 849, -1, 851, 638, 849, -1, 851, 749, 748, -1, 639, 638, 852, -1, 638, 851, 852, -1, 750, 747, 852, -1, 751, 750, 852, -1, 752, 751, 852, -1, 753, 752, 852, -1, 743, 753, 852, -1, 742, 743, 852, -1, 746, 742, 852, -1, 745, 746, 852, -1, 744, 745, 852, -1, 748, 744, 852, -1, 767, 765, 852, -1, 768, 767, 852, -1, 759, 768, 852, -1, 758, 759, 852, -1, 760, 758, 852, -1, 764, 760, 852, -1, 766, 764, 852, -1, 762, 766, 852, -1, 761, 762, 852, -1, 763, 761, 852, -1, 747, 763, 852, -1, 765, 639, 852, -1, 851, 748, 852, -1, 853, 705, 854, -1, 718, 853, 855, -1, 718, 705, 853, -1, 856, 718, 855, -1, 857, 718, 856, -1, 858, 718, 857, -1, 639, 859, 640, -1, 639, 860, 859, -1, 861, 858, 862, -1, 861, 862, 863, -1, 861, 863, 864, -1, 861, 864, 860, -1, 861, 639, 765, -1, 861, 765, 718, -1, 861, 718, 858, -1, 861, 860, 639, -1, 708, 865, 866, -1, 640, 708, 643, -1, 640, 859, 867, -1, 640, 867, 868, -1, 640, 868, 708, -1, 709, 708, 866, -1, 869, 868, 870, -1, 869, 870, 871, -1, 869, 871, 865, -1, 869, 708, 868, -1, 869, 865, 708, -1, 872, 654, 653, -1, 872, 653, 651, -1, 872, 651, 650, -1, 873, 874, 875, -1, 662, 873, 660, -1, 876, 877, 878, -1, 876, 879, 880, -1, 876, 880, 881, -1, 876, 647, 879, -1, 876, 881, 877, -1, 876, 878, 647, -1, 647, 882, 883, -1, 647, 883, 884, -1, 647, 884, 879, -1, 648, 647, 878, -1, 877, 881, 885, -1, 874, 885, 875, -1, 874, 877, 885, -1, 652, 654, 882, -1, 652, 882, 647, -1, 873, 875, 886, -1, 873, 886, 660, -1, 887, 888, 691, -1, 888, 683, 691, -1, 889, 887, 695, -1, 887, 691, 695, -1, 890, 889, 699, -1, 889, 695, 699, -1, 891, 890, 697, -1, 890, 699, 697, -1, 892, 891, 693, -1, 891, 697, 693, -1, 893, 892, 688, -1, 892, 693, 688, -1, 894, 893, 685, -1, 893, 688, 685, -1, 895, 894, 681, -1, 894, 685, 681, -1, 896, 895, 677, -1, 895, 681, 677, -1, 897, 896, 673, -1, 896, 677, 673, -1, 898, 897, 667, -1, 897, 673, 667, -1, 898, 667, 663, -1, 899, 898, 663, -1, 899, 663, 659, -1, 900, 899, 659, -1, 900, 659, 657, -1, 901, 900, 657, -1, 901, 657, 649, -1, 902, 901, 649, -1, 902, 649, 648, -1, 903, 902, 648, -1, 904, 903, 878, -1, 903, 648, 878, -1, 905, 904, 877, -1, 904, 878, 877, -1, 906, 905, 874, -1, 905, 877, 874, -1, 907, 906, 873, -1, 906, 874, 873, -1, 908, 907, 662, -1, 907, 873, 662, -1, 909, 908, 665, -1, 908, 662, 665, -1, 910, 909, 672, -1, 909, 665, 672, -1, 911, 910, 676, -1, 910, 672, 676, -1, 912, 911, 680, -1, 911, 676, 680, -1, 888, 912, 683, -1, 912, 680, 683, -1, 670, 674, 913, -1, 670, 913, 914, -1, 915, 666, 668, -1, 915, 668, 669, -1, 915, 669, 670, -1, 658, 666, 916, -1, 658, 916, 917, -1, 656, 655, 918, -1, 650, 656, 918, -1, 918, 655, 658, -1, 919, 703, 702, -1, 705, 919, 920, -1, 705, 703, 919, -1, 854, 705, 920, -1, 882, 654, 921, -1, 882, 921, 922, -1, 883, 922, 923, -1, 883, 882, 922, -1, 884, 923, 924, -1, 884, 883, 923, -1, 879, 924, 925, -1, 879, 884, 924, -1, 880, 925, 926, -1, 880, 879, 925, -1, 881, 926, 927, -1, 881, 880, 926, -1, 885, 927, 928, -1, 885, 881, 927, -1, 875, 928, 929, -1, 875, 885, 928, -1, 886, 929, 930, -1, 886, 875, 929, -1, 660, 930, 702, -1, 660, 886, 930, -1, 421, 707, 718, -1, 706, 705, 931, -1, 707, 706, 931, -1, 705, 718, 931, -1, 718, 707, 931, -1, 932, 674, 709, -1, 913, 674, 932, -1, 914, 932, 933, -1, 914, 913, 932, -1, 934, 914, 933, -1, 670, 914, 934, -1, 851, 488, 749, -1, 851, 850, 488, -1, 850, 755, 488, -1, 850, 754, 935, -1, 756, 755, 935, -1, 754, 756, 935, -1, 755, 850, 935, -1, 834, 817, 833, -1, 936, 802, 937, -1, 833, 815, 831, -1, 937, 802, 797, -1, 937, 797, 938, -1, 815, 813, 829, -1, 938, 797, 939, -1, 831, 815, 829, -1, 939, 797, 837, -1, 770, 771, 799, -1, 813, 811, 827, -1, 798, 770, 799, -1, 829, 813, 827, -1, 807, 803, 828, -1, 809, 807, 828, -1, 837, 797, 800, -1, 811, 809, 828, -1, 837, 800, 557, -1, 827, 811, 828, -1, 771, 803, 804, -1, 805, 771, 804, -1, 828, 803, 771, -1, 799, 771, 801, -1, 940, 941, 942, -1, 941, 943, 942, -1, 943, 773, 942, -1, 936, 944, 942, -1, 944, 945, 942, -1, 801, 771, 805, -1, 945, 940, 942, -1, 800, 819, 830, -1, 557, 800, 830, -1, 942, 946, 810, -1, 946, 812, 810, -1, 942, 810, 808, -1, 942, 808, 806, -1, 830, 819, 834, -1, 819, 817, 834, -1, 936, 942, 802, -1, 942, 806, 802, -1, 518, 769, 798, -1, 769, 770, 798, -1, 817, 815, 833, -1, 773, 947, 524, -1, 947, 791, 524, -1, 818, 816, 786, -1, 820, 790, 518, -1, 820, 818, 790, -1, 798, 820, 518, -1, 948, 812, 946, -1, 948, 814, 812, -1, 948, 816, 814, -1, 948, 779, 782, -1, 948, 782, 784, -1, 948, 784, 786, -1, 948, 946, 779, -1, 948, 786, 816, -1, 946, 772, 775, -1, 946, 775, 777, -1, 946, 777, 779, -1, 942, 773, 772, -1, 942, 772, 946, -1, 818, 786, 788, -1, 818, 788, 790, -1, 522, 791, 940, -1, 791, 947, 940, -1, 949, 837, 539, -1, 949, 539, 823, -1, 949, 823, 535, -1, 937, 949, 535, -1, 950, 535, 825, -1, 950, 937, 535, -1, 951, 825, 824, -1, 951, 950, 825, -1, 936, 824, 526, -1, 936, 951, 824, -1, 952, 936, 526, -1, 952, 526, 826, -1, 522, 940, 952, -1, 826, 522, 952, -1, 837, 557, 835, -1, 837, 835, 836, -1, 771, 839, 953, -1, 839, 840, 953, -1, 840, 841, 953, -1, 841, 842, 953, -1, 842, 843, 953, -1, 843, 846, 953, -1, 846, 847, 953, -1, 847, 845, 953, -1, 845, 844, 953, -1, 844, 828, 953, -1, 828, 771, 953, -1, 954, 853, 854, -1, 954, 855, 853, -1, 954, 856, 855, -1, 954, 857, 856, -1, 954, 858, 857, -1, 954, 862, 858, -1, 954, 863, 862, -1, 954, 864, 863, -1, 954, 860, 864, -1, 954, 859, 860, -1, 954, 854, 859, -1, 909, 920, 908, -1, 854, 920, 909, -1, 955, 899, 956, -1, 898, 899, 955, -1, 957, 898, 955, -1, 871, 894, 895, -1, 871, 895, 865, -1, 958, 898, 957, -1, 870, 893, 894, -1, 934, 898, 958, -1, 870, 894, 871, -1, 910, 854, 909, -1, 897, 898, 934, -1, 868, 893, 870, -1, 868, 892, 893, -1, 709, 897, 934, -1, 867, 891, 892, -1, 867, 892, 868, -1, 911, 854, 910, -1, 859, 887, 889, -1, 859, 889, 890, -1, 902, 903, 959, -1, 859, 890, 891, -1, 859, 891, 867, -1, 888, 887, 859, -1, 888, 859, 854, -1, 888, 854, 912, -1, 866, 897, 709, -1, 960, 961, 962, -1, 960, 963, 961, -1, 866, 896, 897, -1, 960, 921, 963, -1, 960, 964, 965, -1, 960, 962, 964, -1, 960, 902, 959, -1, 901, 902, 960, -1, 912, 854, 911, -1, 900, 965, 956, -1, 900, 960, 965, -1, 900, 901, 960, -1, 865, 896, 866, -1, 908, 919, 702, -1, 865, 895, 896, -1, 920, 919, 908, -1, 899, 900, 956, -1, 654, 872, 921, -1, 872, 966, 921, -1, 962, 966, 650, -1, 966, 872, 650, -1, 929, 906, 907, -1, 702, 907, 908, -1, 904, 905, 967, -1, 926, 925, 967, -1, 927, 926, 967, -1, 925, 959, 967, -1, 905, 927, 967, -1, 959, 904, 967, -1, 923, 922, 959, -1, 924, 923, 959, -1, 925, 924, 959, -1, 904, 959, 903, -1, 928, 927, 905, -1, 929, 928, 906, -1, 928, 905, 906, -1, 922, 921, 960, -1, 959, 922, 960, -1, 930, 929, 907, -1, 702, 930, 907, -1, 915, 670, 934, -1, 915, 934, 968, -1, 915, 968, 955, -1, 666, 915, 955, -1, 969, 666, 955, -1, 916, 666, 969, -1, 917, 969, 970, -1, 917, 916, 969, -1, 658, 970, 956, -1, 658, 917, 970, -1, 918, 658, 956, -1, 918, 956, 971, -1, 962, 650, 918, -1, 971, 962, 918, -1, 932, 709, 934, -1, 933, 932, 934, -1, 947, 773, 943, -1, 947, 943, 941, -1, 947, 941, 940, -1, 949, 937, 938, -1, 949, 938, 939, -1, 949, 939, 837, -1, 936, 937, 951, -1, 951, 937, 950, -1, 945, 944, 952, -1, 940, 945, 952, -1, 952, 944, 936, -1, 963, 921, 966, -1, 961, 963, 966, -1, 962, 961, 966, -1, 957, 955, 968, -1, 958, 957, 968, -1, 934, 958, 968, -1, 969, 955, 956, -1, 970, 969, 956, -1, 971, 965, 964, -1, 971, 964, 962, -1, 956, 965, 971, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -0.500 2.150 60.778 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -0.500 2.150 -106.278 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -0.500 -81.377 -22.750 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -0.500 85.678 -22.750 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -84.028 2.150 -22.750 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 83.028 2.150 -22.750 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/NECK_Y.wrl000066400000000000000000000063731207742442300226040ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 31.000 30.000 18.500 center 0.000 0.000 0.000 #translation -0.000 -0.000 -9.250 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -15.500 -15.000 0.000, -15.500 15.000 0.000, 15.500 15.000 0.000, 15.500 -15.000 0.000, -15.500 15.000 2.774, -15.500 14.928 3.033, -15.500 -15.000 2.774, -15.500 -14.928 3.033, -15.500 0.000 1.517, 15.500 15.000 2.774, 15.500 14.936 3.005, 15.500 -14.936 3.005, 15.500 -15.000 2.774, 15.500 0.000 1.503, 0.000 -12.819 10.637, -11.500 -10.639 18.500, 11.637 -10.639 18.500, -13.500 0.000 10.767, -11.500 10.639 18.500, 0.000 12.819 10.637, 11.637 10.639 18.500, 13.568 -0.000 10.753 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 3, 0, 2, -1, 4, 1, 5, -1, 0, 6, 7, -1, 1, 0, 8, -1, 7, 5, 8, -1, 5, 1, 8, -1, 0, 7, 8, -1, 9, 1, 4, -1, 9, 2, 1, -1, 10, 2, 9, -1, 11, 12, 3, -1, 13, 3, 2, -1, 13, 10, 11, -1, 13, 2, 10, -1, 13, 11, 3, -1, 6, 0, 12, -1, 0, 3, 12, -1, 14, 7, 6, -1, 14, 15, 7, -1, 14, 16, 15, -1, 14, 11, 16, -1, 14, 12, 11, -1, 14, 6, 12, -1, 17, 18, 5, -1, 17, 15, 18, -1, 17, 7, 15, -1, 17, 5, 7, -1, 9, 4, 19, -1, 4, 5, 19, -1, 5, 18, 19, -1, 18, 20, 19, -1, 20, 10, 19, -1, 10, 9, 19, -1, 21, 10, 20, -1, 21, 11, 10, -1, 21, 16, 11, -1, 21, 20, 16, -1, 16, 18, 15, -1, 20, 18, 16, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 0.000 0.000 56.189 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 0.000 0.000 -37.689 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 0.000 -46.939 9.250 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 0.000 46.939 9.250 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -46.939 0.000 9.250 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 46.939 0.000 9.250 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_ANKLE_P.wrl000066400000000000000000000524351207742442300231660ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 50.000 27.000 39.000 center 0.000 0.000 0.000 #translation 11.000 1.450 -6.500 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.60 0.30 0.30 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -35.000 5.261 -9.070, -35.000 6.376 -7.897, -35.000 11.050 -13.000, -35.000 3.932 -9.995, -35.000 2.445 -10.633, -35.000 0.859 -10.959, -35.000 -0.759 -10.959, -35.000 7.232 -6.523, -35.000 8.040 -3.405, -35.000 7.958 -1.789, -35.000 7.795 -5.005, -35.000 -11.950 -13.000, -35.000 -2.345 -10.633, -35.000 -3.832 -9.995, -35.000 -5.161 -9.070, -35.000 -6.276 -7.897, -35.000 -7.132 -6.523, -35.000 -7.695 -5.005, -35.000 -7.940 -3.405, -35.000 -7.858 -1.789, -11.000 -11.950 26.000, -35.000 -11.950 26.000, -11.000 -11.950 -13.000, -11.000 -13.950 26.000, -11.000 -13.950 -13.000, 13.000 -13.950 -13.000, 5.968 -13.950 -3.501, 5.719 -13.950 -4.472, 5.968 -13.950 -2.499, 5.719 -13.950 -1.527, 13.000 -13.950 26.000, 1.000 -13.950 16.250, 1.000 -13.950 6.500, 4.550 -13.950 0.082, 5.236 -13.950 -0.649, 3.703 -13.950 0.619, 2.750 -13.950 0.929, 1.749 -13.950 0.992, 0.764 -13.950 0.804, -0.143 -13.950 0.377, -0.916 -13.950 -0.262, -1.505 -13.950 -1.073, -1.874 -13.950 -2.005, -1.874 -13.950 -3.995, -2.000 -13.950 -3.000, -1.505 -13.950 -4.927, -0.916 -13.950 -5.738, -0.143 -13.950 -6.377, 0.764 -13.950 -6.804, 1.749 -13.950 -6.992, 4.550 -13.950 -6.082, 3.703 -13.950 -6.619, 2.750 -13.950 -6.929, -0.778 11.050 4.502, -2.232 11.050 3.789, 1.000 11.050 6.500, -11.000 11.050 26.000, 1.000 11.050 16.250, 13.000 11.050 26.000, -3.512 11.050 2.798, -4.566 11.050 1.570, -5.352 11.050 0.155, -5.836 11.050 -1.390, 8.070 11.050 2.211, 6.897 11.050 3.326, 8.995 11.050 0.882, 5.523 11.050 4.182, 9.633 11.050 -0.605, 4.005 11.050 4.745, 9.959 11.050 -2.191, 13.000 11.050 -13.000, 9.633 11.050 -5.395, 8.995 11.050 -6.882, 9.959 11.050 -3.809, 13.000 4.042 -1.749, 13.000 3.854 -0.764, 13.000 3.979 -2.750, 13.000 3.669 -3.703, 13.000 3.132 -4.550, 13.000 -1.450 16.250, 13.000 -1.450 6.500, 13.000 2.788 0.916, 13.000 3.427 0.143, 13.000 0.050 2.000, 13.000 1.045 1.874, 13.000 1.977 1.505, 13.000 -0.945 1.874, 13.000 -2.688 0.916, 13.000 -3.327 0.143, 13.000 -1.877 1.505, 13.000 -3.754 -0.764, 13.000 -0.451 -5.968, 13.000 -1.423 -5.719, 13.000 -2.301 -5.236, 13.000 -3.032 -4.550, 13.000 -3.569 -3.703, 13.000 -3.879 -2.750, 13.000 -3.942 -1.749, 13.000 0.551 -5.968, 13.000 1.522 -5.719, 7.000 -1.450 -13.000, 1.000 -1.450 -13.000, 1.000 4.800 -13.000, -11.000 11.050 -13.000, 1.000 -7.700 -13.000, -5.000 -0.450 -13.000, 13.000 2.401 -5.236, 8.070 11.050 -8.211, 6.897 11.050 -9.326, 5.523 11.050 -10.182, 4.005 11.050 -10.745, 2.405 11.050 -10.990, -6.000 11.050 -3.000, -5.836 11.050 -4.610, -5.352 11.050 -6.155, -4.566 11.050 -7.570, -3.512 11.050 -8.798, -2.232 11.050 -9.789, -0.778 11.050 -10.502, 0.789 11.050 -10.908, 2.405 11.050 4.990, 0.789 11.050 4.908, 5.236 -13.950 -5.351, 14.000 0.050 2.000, 14.000 1.045 1.874, 14.000 1.977 1.505, 14.000 2.788 0.916, 14.000 3.427 0.143, 14.000 3.854 -0.764, 14.000 4.042 -1.749, 14.000 3.979 -2.750, 14.000 3.669 -3.703, 14.000 3.132 -4.550, 14.000 2.401 -5.236, 14.000 1.522 -5.719, 14.000 0.551 -5.968, 14.000 -0.451 -5.968, 14.000 -1.423 -5.719, 14.000 -2.301 -5.236, 14.000 -3.032 -4.550, 14.000 -3.569 -3.703, 14.000 -3.879 -2.750, 14.000 -3.942 -1.749, 14.000 -3.754 -0.764, 14.000 -3.327 0.143, 14.000 -2.688 0.916, 14.000 -1.877 1.505, 14.000 -0.945 1.874, 7.000 -1.450 26.000, 1.000 -1.450 26.000, 1.000 4.800 26.000, 1.000 -7.700 26.000, -5.000 -0.450 26.000, -35.000 11.050 26.000, -6.000 12.050 -3.000, -5.836 12.050 -4.610, -5.352 12.050 -6.155, -4.566 12.050 -7.570, -3.512 12.050 -8.798, -2.232 12.050 -9.789, -0.778 12.050 -10.502, 0.789 12.050 -10.908, 2.405 12.050 -10.990, 4.005 12.050 -10.745, 5.523 12.050 -10.182, 6.897 12.050 -9.326, 8.070 12.050 -8.211, 8.995 12.050 -6.882, 9.633 12.050 -5.395, 9.959 12.050 -3.809, 9.959 12.050 -2.191, 9.633 12.050 -0.605, 8.995 12.050 0.882, 8.070 12.050 2.211, 6.897 12.050 3.326, 5.523 12.050 4.182, 4.005 12.050 4.745, 2.405 12.050 4.990, 0.789 12.050 4.908, -0.778 12.050 4.502, -2.232 12.050 3.789, -3.512 12.050 2.798, -4.566 12.050 1.570, -5.352 12.050 0.155, -5.836 12.050 -1.390, -1.874 -14.950 -3.995, -2.000 -14.950 -3.000, -1.505 -14.950 -4.927, -0.916 -14.950 -5.738, -0.143 -14.950 -6.377, 0.764 -14.950 -6.804, 1.749 -14.950 -6.992, 2.750 -14.950 -6.929, 3.703 -14.950 -6.619, 4.550 -14.950 -6.082, 5.236 -14.950 -5.351, 5.719 -14.950 -4.472, 5.968 -14.950 -3.501, 5.968 -14.950 -2.499, 5.719 -14.950 -1.527, 5.236 -14.950 -0.649, 4.550 -14.950 0.082, 3.703 -14.950 0.619, 2.750 -14.950 0.929, 1.749 -14.950 0.992, 0.764 -14.950 0.804, -0.143 -14.950 0.377, -0.916 -14.950 -0.262, -1.505 -14.950 -1.073, -1.874 -14.950 -2.005, -35.000 -7.452 -0.222, -35.000 4.620 3.566, -35.000 5.848 2.512, -35.000 6.839 1.232, -35.000 3.205 4.352, -35.000 -3.105 4.352, -35.000 -0.450 6.500, -35.000 -1.560 4.836, -35.000 0.050 5.000, -35.000 -0.450 16.250, -35.000 7.552 -0.222, -35.000 -4.520 3.566, -35.000 -5.748 2.512, -35.000 -6.739 1.232, -35.000 1.660 4.836, 14.000 0.050 -1.984, 1.979 12.050 -3.000, 1.984 -14.950 -3.000, -36.000 1.660 4.836, -36.000 0.050 5.000, -36.000 3.205 4.352, -36.000 4.620 3.566, -36.000 5.848 2.512, -36.000 6.839 1.232, -36.000 7.552 -0.222, -36.000 7.958 -1.789, -36.000 8.040 -3.405, -36.000 7.795 -5.005, -36.000 7.232 -6.523, -36.000 6.376 -7.897, -36.000 5.261 -9.070, -36.000 3.932 -9.995, -36.000 2.445 -10.633, -36.000 0.859 -10.959, -36.000 -0.759 -10.959, -36.000 -2.345 -10.633, -36.000 -3.832 -9.995, -36.000 -5.161 -9.070, -36.000 -6.276 -7.897, -36.000 -7.132 -6.523, -36.000 -7.695 -5.005, -36.000 -7.940 -3.405, -36.000 -7.858 -1.789, -36.000 -7.452 -0.222, -36.000 -6.739 1.232, -36.000 -5.748 2.512, -36.000 -4.520 3.566, -36.000 -3.105 4.352, -36.000 -1.560 4.836, -36.000 0.050 -2.979 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 3, 0, 2, -1, 4, 3, 2, -1, 4, 2, 5, -1, 5, 2, 6, -1, 2, 1, 7, -1, 8, 9, 2, -1, 2, 10, 8, -1, 2, 7, 10, -1, 6, 2, 11, -1, 12, 6, 11, -1, 11, 13, 12, -1, 11, 14, 13, -1, 11, 15, 14, -1, 11, 16, 15, -1, 11, 17, 16, -1, 18, 17, 11, -1, 19, 18, 11, -1, 11, 20, 21, -1, 22, 20, 11, -1, 22, 23, 20, -1, 24, 23, 22, -1, 25, 26, 27, -1, 26, 25, 28, -1, 28, 25, 29, -1, 29, 25, 30, -1, 31, 23, 32, -1, 31, 30, 23, -1, 31, 32, 30, -1, 33, 30, 32, -1, 34, 30, 33, -1, 34, 29, 30, -1, 33, 32, 35, -1, 35, 32, 36, -1, 36, 32, 37, -1, 37, 32, 38, -1, 38, 32, 39, -1, 39, 32, 40, -1, 41, 40, 32, -1, 42, 41, 23, -1, 24, 42, 23, -1, 43, 44, 24, -1, 45, 43, 24, -1, 46, 45, 24, -1, 47, 46, 24, -1, 48, 47, 24, -1, 49, 48, 24, -1, 24, 44, 42, -1, 25, 49, 24, -1, 25, 50, 51, -1, 25, 51, 52, -1, 25, 52, 49, -1, 53, 54, 55, -1, 56, 55, 54, -1, 57, 56, 58, -1, 57, 55, 56, -1, 57, 58, 55, -1, 56, 59, 60, -1, 56, 60, 61, -1, 56, 61, 62, -1, 58, 63, 64, -1, 63, 58, 65, -1, 58, 64, 66, -1, 65, 58, 67, -1, 58, 66, 68, -1, 58, 68, 55, -1, 67, 58, 69, -1, 69, 58, 70, -1, 71, 70, 72, -1, 71, 73, 70, -1, 73, 69, 70, -1, 74, 70, 75, -1, 74, 76, 70, -1, 76, 77, 70, -1, 77, 78, 70, -1, 58, 75, 70, -1, 79, 30, 80, -1, 79, 58, 30, -1, 79, 80, 58, -1, 58, 80, 81, -1, 58, 81, 82, -1, 58, 82, 75, -1, 80, 83, 84, -1, 80, 84, 85, -1, 81, 80, 85, -1, 80, 86, 83, -1, 87, 80, 88, -1, 80, 87, 89, -1, 80, 89, 86, -1, 30, 88, 80, -1, 30, 90, 88, -1, 30, 25, 90, -1, 25, 91, 92, -1, 25, 92, 93, -1, 25, 93, 94, -1, 25, 94, 95, -1, 96, 25, 95, -1, 97, 25, 96, -1, 90, 25, 97, -1, 25, 70, 91, -1, 98, 70, 99, -1, 91, 70, 98, -1, 100, 70, 25, -1, 100, 101, 70, -1, 100, 25, 101, -1, 102, 103, 70, -1, 102, 101, 103, -1, 102, 70, 101, -1, 104, 25, 24, -1, 104, 101, 25, -1, 104, 24, 101, -1, 105, 22, 103, -1, 105, 101, 22, -1, 105, 103, 101, -1, 24, 22, 101, -1, 106, 70, 78, -1, 99, 70, 106, -1, 72, 70, 107, -1, 70, 108, 107, -1, 70, 109, 108, -1, 70, 110, 109, -1, 110, 70, 111, -1, 111, 70, 103, -1, 112, 113, 103, -1, 113, 114, 103, -1, 114, 115, 103, -1, 115, 116, 103, -1, 117, 103, 116, -1, 118, 103, 117, -1, 118, 119, 103, -1, 119, 111, 103, -1, 62, 112, 103, -1, 56, 62, 103, -1, 56, 54, 59, -1, 68, 120, 55, -1, 55, 120, 121, -1, 55, 121, 53, -1, 41, 32, 23, -1, 25, 27, 122, -1, 25, 122, 50, -1, 2, 103, 22, -1, 11, 2, 22, -1, 84, 83, 123, -1, 84, 123, 124, -1, 124, 85, 84, -1, 125, 85, 124, -1, 125, 81, 85, -1, 126, 81, 125, -1, 81, 126, 82, -1, 82, 126, 127, -1, 82, 127, 75, -1, 127, 128, 75, -1, 128, 74, 75, -1, 128, 129, 74, -1, 74, 129, 76, -1, 76, 129, 130, -1, 76, 130, 77, -1, 130, 131, 77, -1, 131, 78, 77, -1, 78, 131, 132, -1, 106, 78, 132, -1, 106, 132, 133, -1, 99, 106, 133, -1, 99, 133, 134, -1, 98, 99, 134, -1, 98, 134, 135, -1, 91, 98, 135, -1, 91, 135, 136, -1, 92, 91, 136, -1, 92, 136, 137, -1, 93, 92, 137, -1, 138, 93, 137, -1, 94, 93, 138, -1, 139, 94, 138, -1, 94, 139, 95, -1, 139, 140, 95, -1, 140, 96, 95, -1, 96, 140, 141, -1, 96, 141, 97, -1, 141, 142, 97, -1, 97, 142, 90, -1, 142, 143, 90, -1, 143, 144, 90, -1, 88, 90, 144, -1, 144, 87, 88, -1, 144, 145, 87, -1, 146, 87, 145, -1, 89, 87, 146, -1, 86, 89, 146, -1, 147, 86, 146, -1, 83, 147, 123, -1, 83, 86, 147, -1, 148, 58, 149, -1, 148, 30, 58, -1, 148, 149, 30, -1, 150, 56, 149, -1, 150, 58, 56, -1, 150, 149, 58, -1, 151, 30, 149, -1, 151, 23, 30, -1, 151, 149, 23, -1, 152, 20, 149, -1, 152, 56, 20, -1, 152, 149, 56, -1, 20, 23, 149, -1, 56, 103, 2, -1, 56, 2, 153, -1, 113, 112, 154, -1, 113, 154, 155, -1, 156, 113, 155, -1, 114, 113, 156, -1, 115, 114, 156, -1, 157, 115, 156, -1, 116, 115, 157, -1, 116, 157, 158, -1, 116, 158, 117, -1, 117, 158, 159, -1, 117, 159, 118, -1, 118, 159, 160, -1, 160, 119, 118, -1, 119, 160, 161, -1, 161, 111, 119, -1, 161, 162, 111, -1, 162, 110, 111, -1, 110, 162, 163, -1, 163, 109, 110, -1, 163, 164, 109, -1, 109, 164, 108, -1, 108, 164, 165, -1, 165, 107, 108, -1, 165, 166, 107, -1, 166, 72, 107, -1, 72, 166, 167, -1, 71, 72, 167, -1, 168, 71, 167, -1, 73, 71, 168, -1, 73, 168, 169, -1, 69, 73, 169, -1, 69, 169, 170, -1, 67, 69, 170, -1, 67, 170, 171, -1, 65, 67, 171, -1, 65, 171, 172, -1, 63, 65, 172, -1, 63, 172, 173, -1, 173, 64, 63, -1, 173, 174, 64, -1, 174, 66, 64, -1, 174, 175, 66, -1, 175, 68, 66, -1, 175, 176, 68, -1, 176, 120, 68, -1, 176, 177, 120, -1, 177, 121, 120, -1, 177, 178, 121, -1, 178, 53, 121, -1, 178, 179, 53, -1, 179, 54, 53, -1, 179, 180, 54, -1, 180, 59, 54, -1, 180, 181, 59, -1, 181, 182, 59, -1, 60, 59, 182, -1, 182, 61, 60, -1, 183, 61, 182, -1, 184, 61, 183, -1, 184, 62, 61, -1, 112, 184, 154, -1, 184, 112, 62, -1, 43, 185, 44, -1, 44, 185, 186, -1, 187, 43, 45, -1, 187, 185, 43, -1, 188, 45, 46, -1, 45, 188, 187, -1, 46, 47, 189, -1, 188, 46, 189, -1, 47, 48, 190, -1, 47, 190, 189, -1, 191, 48, 49, -1, 48, 191, 190, -1, 192, 49, 52, -1, 191, 49, 192, -1, 193, 52, 51, -1, 192, 52, 193, -1, 51, 50, 194, -1, 51, 194, 193, -1, 195, 50, 122, -1, 50, 195, 194, -1, 196, 122, 27, -1, 196, 195, 122, -1, 27, 197, 196, -1, 26, 197, 27, -1, 198, 197, 26, -1, 198, 26, 28, -1, 28, 199, 198, -1, 29, 199, 28, -1, 200, 199, 29, -1, 34, 200, 29, -1, 201, 34, 33, -1, 201, 200, 34, -1, 33, 35, 202, -1, 33, 202, 201, -1, 203, 35, 36, -1, 35, 203, 202, -1, 204, 36, 37, -1, 36, 204, 203, -1, 37, 38, 205, -1, 37, 205, 204, -1, 38, 39, 205, -1, 206, 205, 39, -1, 207, 39, 40, -1, 39, 207, 206, -1, 41, 207, 40, -1, 208, 207, 41, -1, 209, 41, 42, -1, 209, 208, 41, -1, 44, 186, 42, -1, 42, 186, 209, -1, 19, 11, 210, -1, 11, 21, 210, -1, 211, 153, 212, -1, 213, 212, 153, -1, 214, 153, 211, -1, 215, 216, 217, -1, 217, 216, 218, -1, 215, 21, 216, -1, 219, 21, 153, -1, 219, 216, 21, -1, 219, 153, 216, -1, 216, 153, 214, -1, 220, 213, 153, -1, 9, 220, 153, -1, 221, 21, 215, -1, 222, 21, 221, -1, 223, 21, 222, -1, 21, 223, 210, -1, 2, 9, 153, -1, 218, 216, 224, -1, 214, 224, 216, -1, 123, 225, 124, -1, 124, 225, 125, -1, 125, 225, 126, -1, 126, 225, 127, -1, 225, 128, 127, -1, 225, 129, 128, -1, 225, 130, 129, -1, 225, 131, 130, -1, 225, 132, 131, -1, 225, 133, 132, -1, 225, 134, 133, -1, 225, 135, 134, -1, 225, 136, 135, -1, 225, 137, 136, -1, 225, 138, 137, -1, 139, 138, 225, -1, 140, 139, 225, -1, 141, 140, 225, -1, 142, 141, 225, -1, 143, 142, 225, -1, 144, 143, 225, -1, 145, 144, 225, -1, 145, 225, 146, -1, 146, 225, 147, -1, 147, 225, 123, -1, 153, 20, 56, -1, 153, 21, 20, -1, 226, 155, 154, -1, 226, 156, 155, -1, 226, 157, 156, -1, 226, 158, 157, -1, 226, 159, 158, -1, 226, 160, 159, -1, 226, 161, 160, -1, 226, 162, 161, -1, 226, 163, 162, -1, 226, 164, 163, -1, 226, 165, 164, -1, 166, 165, 226, -1, 167, 166, 226, -1, 168, 167, 226, -1, 169, 168, 226, -1, 170, 169, 226, -1, 171, 170, 226, -1, 172, 171, 226, -1, 173, 172, 226, -1, 173, 226, 174, -1, 174, 226, 175, -1, 175, 226, 176, -1, 176, 226, 177, -1, 177, 226, 178, -1, 178, 226, 179, -1, 179, 226, 180, -1, 180, 226, 181, -1, 181, 226, 182, -1, 226, 183, 182, -1, 226, 184, 183, -1, 226, 154, 184, -1, 227, 186, 185, -1, 227, 185, 187, -1, 227, 187, 188, -1, 189, 227, 188, -1, 190, 227, 189, -1, 191, 227, 190, -1, 192, 227, 191, -1, 193, 227, 192, -1, 194, 227, 193, -1, 194, 195, 227, -1, 195, 196, 227, -1, 196, 197, 227, -1, 197, 198, 227, -1, 198, 199, 227, -1, 199, 200, 227, -1, 200, 201, 227, -1, 227, 201, 202, -1, 227, 202, 203, -1, 227, 203, 204, -1, 227, 204, 205, -1, 227, 205, 206, -1, 227, 206, 207, -1, 227, 207, 208, -1, 227, 208, 209, -1, 227, 209, 186, -1, 228, 218, 224, -1, 228, 229, 218, -1, 214, 228, 224, -1, 230, 228, 214, -1, 231, 214, 211, -1, 231, 230, 214, -1, 212, 232, 211, -1, 232, 231, 211, -1, 233, 212, 213, -1, 212, 233, 232, -1, 213, 220, 234, -1, 213, 234, 233, -1, 235, 220, 9, -1, 234, 220, 235, -1, 9, 8, 236, -1, 9, 236, 235, -1, 8, 237, 236, -1, 237, 8, 10, -1, 10, 238, 237, -1, 10, 7, 238, -1, 7, 239, 238, -1, 7, 1, 239, -1, 1, 0, 240, -1, 1, 240, 239, -1, 3, 241, 0, -1, 241, 240, 0, -1, 242, 3, 4, -1, 242, 241, 3, -1, 5, 243, 4, -1, 243, 242, 4, -1, 244, 5, 6, -1, 244, 243, 5, -1, 245, 6, 12, -1, 245, 244, 6, -1, 13, 246, 12, -1, 246, 245, 12, -1, 14, 247, 13, -1, 247, 246, 13, -1, 248, 14, 15, -1, 247, 14, 248, -1, 249, 15, 16, -1, 15, 249, 248, -1, 250, 16, 17, -1, 16, 250, 249, -1, 17, 18, 251, -1, 17, 251, 250, -1, 252, 18, 19, -1, 251, 18, 252, -1, 19, 210, 253, -1, 19, 253, 252, -1, 254, 210, 223, -1, 253, 210, 254, -1, 255, 223, 222, -1, 223, 255, 254, -1, 221, 255, 222, -1, 256, 255, 221, -1, 257, 221, 215, -1, 257, 256, 221, -1, 217, 257, 215, -1, 258, 257, 217, -1, 229, 217, 218, -1, 217, 229, 258, -1, 259, 229, 228, -1, 259, 228, 230, -1, 259, 230, 231, -1, 259, 231, 232, -1, 259, 232, 233, -1, 259, 233, 234, -1, 259, 234, 235, -1, 259, 235, 236, -1, 259, 236, 237, -1, 259, 237, 238, -1, 259, 238, 239, -1, 240, 259, 239, -1, 241, 259, 240, -1, 242, 259, 241, -1, 243, 259, 242, -1, 244, 259, 243, -1, 245, 259, 244, -1, 246, 259, 245, -1, 247, 259, 246, -1, 247, 248, 259, -1, 248, 249, 259, -1, 249, 250, 259, -1, 250, 251, 259, -1, 251, 252, 259, -1, 252, 253, 259, -1, 253, 254, 259, -1, 254, 255, 259, -1, 255, 256, 259, -1, 259, 256, 257, -1, 259, 257, 258, -1, 259, 258, 229, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -11.000 -1.450 75.420 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -11.000 -1.450 -62.420 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -11.000 -70.370 6.500 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -11.000 67.470 6.500 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -79.920 -1.450 6.500 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 57.920 -1.450 6.500 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_ANKLE_R.wrl000066400000000000000000000642661207742442300231750ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 85.700 57.080 29.202 center 0.000 0.000 0.000 #translation 12.370 13.610 6.649 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -55.220 8.930 -19.250, -55.220 -36.150 -21.250, -55.220 -36.150 -19.250, -55.220 8.930 -21.250, 30.480 8.930 -19.250, 17.190 12.271 -19.250, 17.190 -12.073 -19.250, -53.463 -40.393 -19.250, -50.773 -41.946 -19.250, -38.810 -12.073 -19.250, -41.110 -12.073 -19.250, -49.220 -42.150 -19.250, 30.480 -36.150 -19.250, -52.220 14.126 -19.250, -53.463 13.173 -19.250, -54.416 11.930 -19.250, 14.890 -12.073 -19.250, 24.480 -42.150 -19.250, -50.773 14.726 -19.250, -49.220 14.930 -19.250, 27.480 -41.346 -19.250, 30.276 -37.703 -19.250, 26.033 -41.946 -19.250, -55.016 10.483 -19.250, 29.676 -39.150 -19.250, 28.723 -40.393 -19.250, 14.890 12.897 -19.250, 14.890 12.930 -19.250, -12.370 -13.610 -19.250, 14.890 12.271 -19.250, -38.810 12.930 -19.250, -38.810 12.897 -19.250, -38.810 12.271 -19.250, 17.190 12.897 -19.250, -41.110 12.930 -19.250, -41.110 12.897 -19.250, 17.190 12.930 -19.250, 24.480 14.930 -19.250, -41.110 12.271 -19.250, 26.033 14.726 -19.250, 27.480 14.126 -19.250, 28.723 13.173 -19.250, 29.676 11.930 -19.250, -55.016 -37.703 -19.250, -54.416 -39.150 -19.250, 30.276 10.483 -19.250, -52.220 -41.346 -19.250, -55.016 10.483 -21.250, -54.416 11.930 -21.250, -53.463 13.173 -21.250, -52.220 14.126 -21.250, -50.773 14.726 -21.250, -49.220 14.930 -21.250, -55.016 -37.703 -21.250, -54.416 -39.150 -21.250, -53.463 -40.393 -21.250, -52.220 -41.346 -21.250, -50.773 -41.946 -21.250, -49.220 -42.150 -21.250, 24.480 14.930 -21.250, 14.890 12.930 -21.250, -38.810 12.930 -21.250, 27.480 14.126 -21.250, 26.033 14.726 -21.250, 29.676 11.930 -21.250, 28.723 13.173 -21.250, 30.480 8.930 -21.250, 30.276 10.483 -21.250, 17.190 12.930 -21.250, -41.110 12.930 -21.250, -41.110 -12.930 -21.250, 17.190 -12.930 -21.250, 30.480 -36.150 -21.250, 24.480 -42.150 -21.250, 14.890 -12.930 -21.250, 27.480 -41.346 -21.250, 30.276 -37.703 -21.250, 26.033 -41.946 -21.250, 29.676 -39.150 -21.250, -38.810 -12.930 -21.250, 28.723 -40.393 -21.250, -12.370 -13.610 -21.250, 14.890 12.930 -19.243, 17.190 12.930 -19.243, 17.190 7.991 0.381, 14.890 7.991 0.381, 17.190 3.569 -7.160, 17.190 0.099 -9.379, 17.190 -5.151 -6.121, 17.190 -6.426 -4.765, 17.190 -3.597 -7.146, 17.190 -7.353 -3.151, 17.190 -7.882 -1.366, 17.190 -7.985 0.492, 17.190 5.114 -6.152, 17.190 6.388 -4.816, 17.190 7.322 -3.224, 17.190 7.866 -1.461, 17.190 1.833 -7.787, 17.190 0.000 -8.000, 17.190 -1.849 -7.783, 14.890 -7.985 0.492, 14.890 0.099 -9.379, 14.890 3.569 -7.160, 14.890 -6.426 -4.765, 14.890 -5.151 -6.121, 14.890 -3.597 -7.146, 14.890 -7.353 -3.151, 14.890 -7.882 -1.366, 14.890 5.114 -6.152, 14.890 6.388 -4.816, 14.890 7.322 -3.224, 14.890 7.866 -1.461, 14.890 1.833 -7.787, 14.890 0.000 -8.000, 14.890 -1.849 -7.783, -38.810 12.930 -19.243, -41.110 12.930 -19.243, -38.810 7.991 0.381, -41.110 7.991 0.381, -38.810 -5.151 -6.121, -38.810 -6.426 -4.765, -38.810 -3.597 -7.146, -38.810 -7.353 -3.151, -38.810 -7.882 -1.366, -38.810 -7.985 0.492, -38.810 3.569 -7.160, -38.810 5.114 -6.152, -38.810 6.388 -4.816, -38.810 7.322 -3.224, -38.810 7.866 -1.461, -38.810 7.991 0.381, -38.810 0.099 -9.379, -38.810 1.833 -7.787, -38.810 0.000 -8.000, -38.810 -1.849 -7.783, -41.110 -6.426 -4.765, -41.110 -5.151 -6.121, -41.110 -3.597 -7.146, -41.110 -7.353 -3.151, -41.110 -7.882 -1.366, -41.110 -7.985 0.492, -41.110 5.114 -6.152, -41.110 3.569 -7.160, -41.110 6.388 -4.816, -41.110 7.322 -3.224, -41.110 7.991 0.381, -41.110 7.866 -1.461, -41.110 0.099 -9.379, -41.110 1.833 -7.787, -41.110 0.000 -8.000, -41.110 -1.849 -7.783, 14.890 4.452 6.647, 17.190 4.452 6.647, 17.190 5.868 5.438, 14.890 6.966 3.934, 14.890 5.868 5.438, 17.190 6.966 3.934, 14.890 -7.655 2.325, 17.190 -7.655 2.325, 14.890 7.687 2.218, 17.190 7.687 2.218, 14.890 -6.910 4.031, 17.190 -6.910 4.031, 14.890 -5.791 5.519, 17.190 -5.791 5.519, 14.890 -4.358 6.708, 17.190 -4.358 6.708, 14.890 -2.690 7.534, 17.190 -2.690 7.534, 14.890 -0.875 7.952, 17.190 -0.875 7.952, 14.890 0.986 7.939, 17.190 0.986 7.939, 14.890 2.795 7.496, 17.190 2.795 7.496, 17.190 7.638 2.000, 17.190 3.464 2.000, 17.190 3.864 1.035, 17.190 -3.864 1.035, 17.190 -3.464 2.000, 17.190 -7.673 2.000, 17.190 4.000 0.000, 17.190 1.035 -3.864, 17.190 0.000 -4.000, 17.190 3.864 -1.035, 17.190 -4.000 0.000, 17.190 3.464 -2.000, 17.190 -3.864 -1.035, 17.190 2.828 -2.828, 17.190 -1.035 -3.864, 17.190 2.000 -3.464, 17.190 -3.464 -2.000, 17.190 -2.000 -3.464, 17.190 -2.828 -2.828, 14.890 3.864 1.035, 14.890 3.464 2.000, 14.890 7.638 2.000, 14.890 4.000 0.000, 14.890 3.864 -1.035, 14.890 -7.673 2.000, 14.890 -3.464 2.000, 14.890 -3.864 1.035, 14.890 0.000 -4.000, 14.890 1.035 -3.864, 14.890 3.464 -2.000, 14.890 -4.000 0.000, 14.890 2.828 -2.828, 14.890 -3.864 -1.035, 14.890 2.000 -3.464, 14.890 -1.035 -3.864, 14.890 -3.464 -2.000, 14.890 -2.000 -3.464, 14.890 -2.828 -2.828, -41.110 6.966 3.934, -41.110 5.868 5.438, -38.810 6.966 3.934, -38.810 5.868 5.438, -41.110 -7.655 2.325, -41.110 7.687 2.218, -38.810 7.687 2.218, -38.810 -7.655 2.325, -41.110 -6.910 4.031, -38.810 -6.910 4.031, -41.110 -5.791 5.519, -38.810 -5.791 5.519, -41.110 -4.358 6.708, -38.810 -4.358 6.708, -41.110 -2.690 7.534, -38.810 -2.690 7.534, -41.110 -0.875 7.952, -38.810 -0.875 7.952, -41.110 0.986 7.939, -38.810 0.986 7.939, -41.110 2.795 7.496, -38.810 2.795 7.496, -41.110 4.452 6.647, -38.810 4.452 6.647, -38.810 7.638 2.000, -38.810 3.464 2.000, -38.810 3.864 1.035, -38.810 -3.864 1.035, -38.810 -3.464 2.000, -38.810 -7.673 2.000, -38.810 1.035 -3.864, -38.810 0.000 -4.000, -38.810 4.000 0.000, -38.810 -4.000 -0.000, -38.810 3.864 -1.035, -38.810 3.464 -2.000, -38.810 -3.864 -1.035, -38.810 -1.035 -3.864, -38.810 2.828 -2.828, -38.810 -3.464 -2.000, -38.810 2.000 -3.464, -38.810 -2.000 -3.464, -38.810 -2.828 -2.828, -41.110 -7.673 2.000, -41.110 -3.464 2.000, -41.110 -3.864 1.035, -41.110 3.864 1.035, -41.110 3.464 2.000, -41.110 7.638 2.000, -41.110 0.000 -4.000, -41.110 1.035 -3.864, -41.110 4.000 0.000, -41.110 -4.000 -0.000, -41.110 3.864 -1.035, -41.110 -3.864 -1.035, -41.110 3.464 -2.000, -41.110 -1.035 -3.864, -41.110 2.828 -2.828, -41.110 -3.464 -2.000, -41.110 -2.000 -3.464, -41.110 2.000 -3.464, -41.110 -2.828 -2.828, 14.890 -2.000 3.464, 14.890 -4.358 6.708, 14.890 -2.690 7.534, 14.890 -1.035 3.864, 14.890 -2.828 2.828, 14.890 6.966 3.934, 14.890 7.687 2.218, 14.890 0.003 4.166, 14.890 0.000 4.000, 14.890 1.035 3.864, 14.890 -0.875 7.952, 14.890 0.986 7.939, 14.890 2.828 2.828, 14.890 5.868 5.438, 14.890 2.000 3.464, 14.890 2.795 7.496, 17.190 -1.035 3.864, 17.190 -2.690 7.534, 17.190 -2.000 3.464, 17.190 -4.358 6.708, 17.190 -2.828 2.828, 17.190 7.687 2.218, 17.190 6.966 3.934, 17.190 0.000 4.000, 17.190 0.003 4.166, 17.190 1.035 3.864, 17.190 0.986 7.939, 17.190 -0.875 7.952, 17.190 5.868 5.438, 17.190 2.828 2.828, 17.190 2.795 7.496, 17.190 2.000 3.464, -41.110 -2.000 3.464, -41.110 -2.690 7.534, -41.110 -1.035 3.864, -41.110 -2.828 2.828, -41.110 -4.358 6.708, -41.110 -5.791 5.519, -41.110 -6.910 4.031, -41.110 7.687 2.218, -41.110 0.003 4.166, -41.110 -0.000 4.000, -41.110 1.035 3.864, -41.110 -0.875 7.952, -41.110 0.986 7.939, -41.110 2.828 2.828, -41.110 4.452 6.647, -41.110 5.868 5.438, -41.110 2.000 3.464, -41.110 2.795 7.496, -38.810 -1.035 3.864, -38.810 -2.690 7.534, -38.810 -2.000 3.464, -38.810 -4.358 6.708, -38.810 -2.828 2.828, -38.810 -5.791 5.519, -38.810 -6.910 4.031, -38.810 7.687 2.218, -38.810 -0.000 4.000, -38.810 0.003 4.166, -38.810 1.035 3.864, -38.810 0.986 7.939, -38.810 -0.875 7.952, -38.810 5.868 5.438, -38.810 4.452 6.647, -38.810 2.828 2.828, -38.810 2.795 7.496, -38.810 2.000 3.464 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 0, 3, 1, -1, 4, 5, 6, -1, 2, 7, 8, -1, 9, 10, 11, -1, 10, 2, 11, -1, 4, 6, 12, -1, 2, 8, 11, -1, 13, 14, 15, -1, 6, 16, 17, -1, 12, 6, 17, -1, 18, 13, 19, -1, 12, 17, 20, -1, 21, 12, 20, -1, 17, 22, 20, -1, 15, 23, 0, -1, 21, 20, 24, -1, 13, 15, 0, -1, 19, 13, 0, -1, 24, 20, 25, -1, 26, 27, 28, -1, 16, 29, 28, -1, 30, 31, 28, -1, 32, 9, 28, -1, 17, 16, 28, -1, 26, 29, 33, -1, 11, 17, 28, -1, 9, 11, 28, -1, 33, 29, 5, -1, 31, 32, 28, -1, 34, 19, 35, -1, 27, 30, 28, -1, 29, 26, 28, -1, 27, 36, 37, -1, 19, 0, 38, -1, 33, 5, 37, -1, 36, 33, 37, -1, 35, 19, 38, -1, 19, 30, 37, -1, 30, 27, 37, -1, 19, 34, 30, -1, 39, 37, 40, -1, 31, 35, 32, -1, 35, 38, 32, -1, 41, 40, 42, -1, 38, 0, 10, -1, 10, 0, 2, -1, 42, 40, 4, -1, 40, 37, 4, -1, 2, 43, 44, -1, 45, 42, 4, -1, 37, 5, 4, -1, 2, 44, 7, -1, 7, 46, 8, -1, 47, 3, 23, -1, 3, 0, 23, -1, 48, 47, 15, -1, 47, 23, 15, -1, 49, 48, 14, -1, 48, 15, 14, -1, 50, 49, 13, -1, 49, 14, 13, -1, 51, 50, 18, -1, 50, 13, 18, -1, 52, 51, 19, -1, 51, 18, 19, -1, 43, 1, 53, -1, 43, 2, 1, -1, 44, 53, 54, -1, 44, 43, 53, -1, 7, 54, 55, -1, 7, 44, 54, -1, 46, 55, 56, -1, 46, 7, 55, -1, 8, 56, 57, -1, 8, 46, 56, -1, 11, 57, 58, -1, 11, 8, 57, -1, 59, 60, 61, -1, 62, 59, 63, -1, 48, 49, 50, -1, 64, 62, 65, -1, 52, 50, 51, -1, 3, 47, 48, -1, 3, 48, 50, -1, 66, 64, 67, -1, 3, 50, 52, -1, 66, 68, 59, -1, 66, 59, 62, -1, 69, 3, 52, -1, 66, 62, 64, -1, 61, 69, 52, -1, 70, 3, 69, -1, 71, 68, 66, -1, 1, 3, 70, -1, 72, 71, 66, -1, 73, 74, 71, -1, 73, 71, 72, -1, 54, 53, 1, -1, 55, 54, 1, -1, 75, 73, 72, -1, 75, 72, 76, -1, 75, 77, 73, -1, 57, 56, 55, -1, 78, 75, 76, -1, 57, 55, 1, -1, 58, 70, 79, -1, 58, 1, 70, -1, 80, 75, 78, -1, 81, 60, 74, -1, 58, 57, 1, -1, 81, 79, 61, -1, 81, 73, 58, -1, 81, 61, 60, -1, 81, 58, 79, -1, 81, 74, 73, -1, 59, 68, 60, -1, 59, 61, 52, -1, 36, 27, 82, -1, 83, 36, 82, -1, 33, 36, 83, -1, 82, 26, 83, -1, 83, 26, 33, -1, 82, 27, 26, -1, 84, 29, 85, -1, 84, 5, 29, -1, 5, 86, 87, -1, 6, 88, 89, -1, 88, 6, 90, -1, 6, 89, 91, -1, 6, 91, 92, -1, 6, 92, 93, -1, 86, 5, 94, -1, 94, 5, 95, -1, 95, 5, 96, -1, 97, 5, 84, -1, 96, 5, 97, -1, 6, 5, 87, -1, 86, 98, 87, -1, 98, 99, 87, -1, 99, 100, 87, -1, 100, 90, 87, -1, 90, 6, 87, -1, 101, 16, 93, -1, 16, 6, 93, -1, 102, 103, 29, -1, 104, 105, 16, -1, 106, 16, 105, -1, 107, 104, 16, -1, 108, 107, 16, -1, 101, 108, 16, -1, 109, 29, 103, -1, 110, 29, 109, -1, 111, 29, 110, -1, 85, 29, 112, -1, 112, 29, 111, -1, 102, 29, 16, -1, 102, 113, 103, -1, 102, 114, 113, -1, 102, 115, 114, -1, 102, 106, 115, -1, 102, 16, 106, -1, 116, 34, 117, -1, 116, 30, 34, -1, 31, 30, 116, -1, 117, 34, 35, -1, 116, 117, 31, -1, 117, 35, 31, -1, 118, 38, 119, -1, 118, 32, 38, -1, 9, 120, 121, -1, 120, 9, 122, -1, 9, 121, 123, -1, 9, 123, 124, -1, 9, 124, 125, -1, 126, 32, 127, -1, 127, 32, 128, -1, 128, 32, 129, -1, 130, 32, 131, -1, 129, 32, 130, -1, 9, 32, 132, -1, 126, 133, 132, -1, 133, 134, 132, -1, 134, 135, 132, -1, 135, 122, 132, -1, 122, 9, 132, -1, 32, 126, 132, -1, 136, 137, 10, -1, 138, 10, 137, -1, 139, 136, 10, -1, 140, 139, 10, -1, 141, 140, 10, -1, 142, 38, 143, -1, 144, 38, 142, -1, 145, 38, 144, -1, 146, 38, 147, -1, 147, 38, 145, -1, 148, 38, 10, -1, 148, 149, 143, -1, 148, 150, 149, -1, 148, 151, 150, -1, 148, 138, 151, -1, 148, 10, 138, -1, 148, 143, 38, -1, 141, 10, 125, -1, 10, 9, 125, -1, 37, 59, 52, -1, 37, 52, 19, -1, 58, 73, 17, -1, 11, 58, 17, -1, 45, 66, 67, -1, 45, 4, 66, -1, 42, 67, 64, -1, 42, 45, 67, -1, 41, 64, 65, -1, 41, 42, 64, -1, 40, 65, 62, -1, 40, 41, 65, -1, 39, 62, 63, -1, 39, 40, 62, -1, 37, 63, 59, -1, 37, 39, 63, -1, 76, 72, 21, -1, 72, 12, 21, -1, 78, 76, 24, -1, 76, 21, 24, -1, 80, 78, 25, -1, 78, 24, 25, -1, 75, 80, 20, -1, 80, 25, 20, -1, 77, 75, 22, -1, 75, 20, 22, -1, 73, 77, 17, -1, 77, 22, 17, -1, 12, 72, 4, -1, 72, 66, 4, -1, 71, 60, 68, -1, 71, 74, 60, -1, 79, 69, 61, -1, 79, 70, 69, -1, 152, 153, 154, -1, 155, 156, 157, -1, 101, 93, 158, -1, 156, 154, 157, -1, 158, 93, 159, -1, 160, 155, 161, -1, 155, 157, 161, -1, 162, 158, 163, -1, 85, 160, 84, -1, 158, 159, 163, -1, 160, 161, 84, -1, 164, 162, 165, -1, 162, 163, 165, -1, 166, 164, 167, -1, 164, 165, 167, -1, 168, 166, 169, -1, 166, 167, 169, -1, 170, 168, 171, -1, 168, 169, 171, -1, 172, 170, 173, -1, 170, 171, 173, -1, 174, 172, 175, -1, 172, 173, 175, -1, 152, 174, 153, -1, 174, 175, 153, -1, 156, 152, 154, -1, 176, 177, 178, -1, 179, 180, 181, -1, 97, 84, 182, -1, 183, 184, 98, -1, 84, 176, 182, -1, 176, 178, 182, -1, 86, 183, 98, -1, 96, 97, 185, -1, 97, 182, 185, -1, 186, 179, 93, -1, 179, 181, 93, -1, 95, 96, 187, -1, 98, 184, 99, -1, 96, 185, 187, -1, 188, 186, 92, -1, 186, 93, 92, -1, 95, 187, 189, -1, 184, 190, 100, -1, 95, 189, 94, -1, 99, 184, 100, -1, 94, 189, 191, -1, 192, 188, 91, -1, 188, 92, 91, -1, 190, 193, 90, -1, 100, 190, 90, -1, 194, 192, 89, -1, 192, 91, 89, -1, 191, 183, 86, -1, 194, 89, 88, -1, 193, 194, 88, -1, 90, 193, 88, -1, 94, 191, 86, -1, 195, 196, 197, -1, 198, 85, 112, -1, 198, 197, 85, -1, 198, 195, 197, -1, 199, 112, 111, -1, 200, 201, 202, -1, 199, 198, 112, -1, 113, 203, 204, -1, 205, 111, 110, -1, 113, 204, 103, -1, 205, 199, 111, -1, 101, 202, 206, -1, 101, 200, 202, -1, 207, 205, 110, -1, 109, 207, 110, -1, 114, 203, 113, -1, 108, 206, 208, -1, 108, 101, 206, -1, 209, 207, 109, -1, 115, 210, 203, -1, 115, 203, 114, -1, 107, 208, 211, -1, 107, 108, 208, -1, 106, 212, 210, -1, 103, 204, 209, -1, 106, 210, 115, -1, 103, 209, 109, -1, 104, 211, 213, -1, 104, 107, 211, -1, 105, 104, 213, -1, 105, 213, 212, -1, 105, 212, 106, -1, 214, 215, 216, -1, 215, 217, 216, -1, 141, 125, 218, -1, 219, 214, 220, -1, 218, 125, 221, -1, 214, 216, 220, -1, 218, 221, 222, -1, 119, 219, 118, -1, 222, 221, 223, -1, 219, 220, 118, -1, 224, 222, 225, -1, 222, 223, 225, -1, 226, 224, 227, -1, 224, 225, 227, -1, 228, 226, 229, -1, 226, 227, 229, -1, 230, 228, 231, -1, 228, 229, 231, -1, 232, 230, 233, -1, 230, 231, 233, -1, 234, 232, 235, -1, 232, 233, 235, -1, 236, 234, 237, -1, 234, 235, 237, -1, 215, 236, 217, -1, 236, 237, 217, -1, 238, 239, 240, -1, 241, 242, 243, -1, 244, 245, 133, -1, 130, 131, 246, -1, 126, 244, 133, -1, 131, 238, 246, -1, 238, 240, 246, -1, 247, 241, 125, -1, 241, 243, 125, -1, 129, 130, 248, -1, 130, 246, 248, -1, 133, 245, 134, -1, 128, 129, 249, -1, 129, 248, 249, -1, 250, 247, 124, -1, 247, 125, 124, -1, 245, 251, 135, -1, 128, 249, 252, -1, 128, 252, 127, -1, 134, 245, 135, -1, 253, 250, 123, -1, 250, 124, 123, -1, 127, 252, 254, -1, 251, 255, 122, -1, 135, 251, 122, -1, 256, 253, 121, -1, 253, 123, 121, -1, 256, 121, 120, -1, 255, 256, 120, -1, 254, 244, 126, -1, 122, 255, 120, -1, 127, 254, 126, -1, 257, 258, 259, -1, 260, 261, 262, -1, 149, 263, 264, -1, 149, 264, 143, -1, 265, 146, 147, -1, 265, 262, 146, -1, 265, 260, 262, -1, 141, 259, 266, -1, 141, 257, 259, -1, 267, 147, 145, -1, 267, 265, 147, -1, 150, 263, 149, -1, 140, 266, 268, -1, 269, 145, 144, -1, 140, 141, 266, -1, 269, 267, 145, -1, 151, 270, 263, -1, 271, 269, 144, -1, 151, 263, 150, -1, 142, 271, 144, -1, 139, 268, 272, -1, 139, 140, 268, -1, 138, 273, 270, -1, 274, 271, 142, -1, 138, 270, 151, -1, 136, 272, 275, -1, 136, 139, 272, -1, 137, 136, 275, -1, 143, 264, 274, -1, 137, 275, 273, -1, 137, 273, 138, -1, 143, 274, 142, -1, 276, 277, 278, -1, 276, 278, 279, -1, 280, 277, 276, -1, 164, 277, 280, -1, 201, 164, 280, -1, 162, 164, 201, -1, 197, 281, 282, -1, 158, 162, 201, -1, 200, 158, 201, -1, 85, 197, 282, -1, 101, 158, 200, -1, 283, 284, 279, -1, 283, 285, 284, -1, 283, 286, 287, -1, 283, 287, 285, -1, 283, 279, 286, -1, 288, 152, 289, -1, 290, 291, 152, -1, 290, 152, 288, -1, 196, 289, 281, -1, 196, 281, 197, -1, 196, 288, 289, -1, 285, 287, 291, -1, 285, 291, 290, -1, 279, 278, 286, -1, 292, 293, 294, -1, 294, 295, 296, -1, 296, 295, 165, -1, 296, 165, 180, -1, 180, 165, 163, -1, 297, 298, 176, -1, 180, 163, 159, -1, 180, 159, 181, -1, 181, 159, 93, -1, 297, 176, 84, -1, 292, 299, 300, -1, 299, 301, 300, -1, 302, 303, 300, -1, 301, 302, 300, -1, 303, 292, 300, -1, 304, 153, 305, -1, 153, 306, 307, -1, 305, 153, 307, -1, 298, 304, 177, -1, 176, 298, 177, -1, 304, 305, 177, -1, 306, 302, 301, -1, 307, 306, 301, -1, 303, 293, 292, -1, 293, 295, 294, -1, 189, 205, 207, -1, 294, 280, 276, -1, 189, 187, 205, -1, 294, 296, 280, -1, 210, 184, 203, -1, 191, 207, 209, -1, 191, 189, 207, -1, 292, 276, 279, -1, 292, 294, 276, -1, 190, 184, 210, -1, 183, 209, 204, -1, 183, 191, 209, -1, 299, 279, 284, -1, 193, 210, 212, -1, 299, 292, 279, -1, 193, 190, 210, -1, 184, 204, 203, -1, 184, 183, 204, -1, 301, 284, 285, -1, 194, 212, 213, -1, 301, 299, 284, -1, 194, 193, 212, -1, 307, 301, 285, -1, 307, 285, 290, -1, 192, 213, 211, -1, 192, 194, 213, -1, 305, 307, 290, -1, 305, 290, 288, -1, 188, 211, 208, -1, 188, 192, 211, -1, 177, 305, 288, -1, 177, 288, 196, -1, 206, 188, 208, -1, 178, 196, 195, -1, 186, 188, 206, -1, 178, 177, 196, -1, 179, 206, 202, -1, 182, 195, 198, -1, 182, 178, 195, -1, 179, 186, 206, -1, 180, 202, 201, -1, 185, 198, 199, -1, 185, 182, 198, -1, 180, 179, 202, -1, 296, 201, 280, -1, 187, 199, 205, -1, 187, 185, 199, -1, 296, 180, 201, -1, 308, 309, 310, -1, 311, 312, 308, -1, 313, 312, 311, -1, 258, 313, 311, -1, 314, 313, 258, -1, 218, 314, 258, -1, 262, 214, 315, -1, 257, 218, 258, -1, 141, 218, 257, -1, 316, 317, 310, -1, 316, 318, 317, -1, 316, 319, 320, -1, 316, 320, 318, -1, 146, 262, 315, -1, 316, 310, 319, -1, 321, 322, 323, -1, 261, 323, 214, -1, 261, 214, 262, -1, 261, 321, 323, -1, 324, 325, 322, -1, 324, 322, 321, -1, 318, 320, 325, -1, 318, 325, 324, -1, 310, 309, 319, -1, 308, 312, 309, -1, 326, 327, 328, -1, 328, 329, 330, -1, 330, 329, 331, -1, 330, 331, 242, -1, 242, 331, 332, -1, 242, 332, 221, -1, 333, 216, 238, -1, 242, 221, 243, -1, 243, 221, 125, -1, 326, 334, 335, -1, 334, 336, 335, -1, 337, 338, 335, -1, 336, 337, 335, -1, 333, 238, 131, -1, 338, 326, 335, -1, 339, 340, 341, -1, 340, 342, 343, -1, 341, 340, 343, -1, 216, 339, 239, -1, 238, 216, 239, -1, 339, 341, 239, -1, 342, 337, 336, -1, 343, 342, 336, -1, 338, 327, 326, -1, 327, 329, 328, -1, 254, 271, 274, -1, 328, 330, 311, -1, 254, 252, 271, -1, 310, 328, 308, -1, 270, 245, 263, -1, 244, 274, 264, -1, 244, 254, 274, -1, 326, 328, 310, -1, 317, 326, 310, -1, 251, 245, 270, -1, 245, 264, 263, -1, 245, 244, 264, -1, 334, 326, 317, -1, 273, 251, 270, -1, 318, 334, 317, -1, 255, 251, 273, -1, 336, 334, 318, -1, 324, 336, 318, -1, 275, 255, 273, -1, 256, 255, 275, -1, 343, 336, 324, -1, 321, 343, 324, -1, 253, 275, 272, -1, 341, 343, 321, -1, 253, 256, 275, -1, 261, 341, 321, -1, 250, 272, 268, -1, 239, 341, 261, -1, 250, 253, 272, -1, 240, 261, 260, -1, 240, 239, 261, -1, 266, 250, 268, -1, 247, 250, 266, -1, 246, 260, 265, -1, 246, 240, 260, -1, 241, 266, 259, -1, 241, 247, 266, -1, 248, 265, 267, -1, 248, 246, 265, -1, 242, 259, 258, -1, 242, 241, 259, -1, 249, 267, 269, -1, 249, 248, 267, -1, 330, 258, 311, -1, 330, 242, 258, -1, 252, 269, 271, -1, 252, 249, 269, -1, 328, 311, 308, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -12.370 -13.610 100.381 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -12.370 -13.610 -113.679 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -12.370 -120.640 -6.649 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -12.370 93.420 -6.649 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -119.400 -13.610 -6.649 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 94.660 -13.610 -6.649 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_ELBOW_P.wrl000066400000000000000000001735621207742442300232110ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 43.000 28.000 79.000 center 0.000 0.000 0.000 translation 0.000 -0.000 0.000 #translation -10.500 -0.500 29.500 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 31.000 -7.000 -65.268, 31.000 -7.000 -68.000, 29.500 -7.000 -67.002, 30.000 -7.000 -68.000, 30.649 -7.000 -65.108, 25.106 -9.500 -61.032, 25.089 -9.500 -61.024, 25.070 -9.500 -61.047, 25.142 -9.500 -61.016, 25.177 -9.500 -61.000, 28.296 -9.500 -65.953, 28.529 -9.500 -65.645, 23.500 -9.500 -65.024, 28.127 -9.500 -66.299, 28.027 -9.500 -66.671, 28.001 -9.500 -67.055, 28.048 -9.500 -67.437, 28.168 -9.500 -67.803, 28.356 -9.500 -68.140, 28.605 -9.500 -68.434, 16.080 -9.500 -63.629, 28.816 7.500 -65.388, 24.946 7.500 -65.024, 28.529 7.500 -65.645, 28.296 7.500 -65.953, 28.127 7.500 -66.299, 24.871 7.500 -65.636, 25.106 -7.000 -61.032, 25.070 -7.000 -61.047, 25.089 -7.000 -61.024, 25.142 -7.000 -61.016, 25.177 -7.000 -61.000, 28.605 -7.000 -68.434, 24.310 -7.000 -66.956, 28.356 -7.000 -68.140, 28.906 -7.000 -68.674, 28.168 -7.000 -67.803, 29.371 -7.000 -69.000, 29.247 -7.000 -68.853, 28.048 -7.000 -67.437, 29.617 -7.000 -68.963, 28.001 -7.000 -67.055, 24.871 -7.000 -65.636, 30.000 -7.000 -69.000, 28.027 -7.000 -66.671, 28.127 -7.000 -66.299, 29.146 -7.000 -65.191, 28.816 -7.000 -65.388, 29.509 -7.000 -65.061, 29.890 -7.000 -65.003, 30.275 -7.000 -65.019, 31.000 -7.000 -65.047, 24.946 -7.000 -65.024, 28.529 -7.000 -65.645, 28.296 -7.000 -65.953, 20.790 -7.000 -65.533, 24.285 -8.500 -67.017, 20.790 -8.500 -65.533, 20.142 -0.500 -65.258, 31.000 10.000 -65.268, 31.000 10.000 -68.000, 29.500 10.000 -67.002, 30.000 10.000 -68.000, 30.649 10.000 -65.108, 24.310 7.500 -66.956, 29.371 10.000 -69.000, 29.371 7.500 -69.000, 22.685 8.750 -66.299, 23.055 -7.000 -61.858, 25.000 -7.000 -61.000, 20.552 -7.000 -62.049, 16.000 7.500 -63.342, 16.044 7.500 -63.396, 16.000 -7.000 -63.342, 16.044 -7.000 -63.396, 2.522 0.250 -46.698, 25.106 10.000 -61.032, 25.070 10.000 -61.047, 25.089 10.000 -61.024, 25.142 10.000 -61.016, 25.177 10.000 -61.000, 25.000 10.000 -61.000, 29.146 10.000 -65.191, 28.816 10.000 -65.388, 28.529 10.000 -65.645, 29.509 10.000 -65.061, 29.890 10.000 -65.003, 30.275 10.000 -65.019, 31.000 10.000 -65.047, 23.500 10.000 -65.024, 2.500 -7.000 -46.766, -11.000 -7.000 -30.000, 16.000 -7.000 -63.500, 16.000 -7.000 -63.531, -10.954 -7.000 -30.246, 25.000 -9.500 -61.000, 7.000 -9.500 -46.766, 16.000 -9.500 -61.000, 16.000 -9.500 -63.531, -10.954 -9.500 -30.246, 11.841 -9.500 -30.000, -9.070 -7.000 -30.000, 16.033 -7.000 -61.000, 3.645 0.250 -45.702, -9.070 7.500 -30.000, 6.313 13.500 4.914, 0.866 13.500 0.500, 0.707 13.500 0.707, 6.815 13.500 4.189, 7.235 13.500 3.414, 0.966 13.500 0.259, 7.567 13.500 2.598, 7.951 13.500 0.880, 1.000 13.500 -0.000, 8.000 13.500 0.000, 7.235 13.500 -3.414, 0.866 13.500 -0.500, 0.966 13.500 -0.259, 6.815 13.500 -4.189, 6.313 13.500 -4.914, 0.707 13.500 -0.707, 5.734 13.500 -5.578, 11.262 7.500 -29.965, 10.857 7.500 -29.500, 11.000 7.500 -30.000, 11.506 7.500 -29.863, 10.741 7.500 -29.966, 11.000 -9.500 -30.000, 0.000 -9.500 -29.500, -11.000 -9.500 -30.000, -11.000 -9.500 -29.000, 11.262 -9.500 -29.965, 10.857 -9.500 -29.500, 11.506 -9.500 -29.863, 10.741 -9.500 -29.966, 10.034 -9.500 -29.259, 10.134 -9.500 -29.500, 10.293 -9.500 -29.707, 10.500 -9.500 -29.866, 11.000 -9.500 -29.000, 10.034 7.500 -29.259, 10.000 7.500 -29.000, -0.000 7.500 -29.500, 10.134 7.500 -29.500, 10.293 7.500 -29.707, 10.500 7.500 -29.866, -9.834 7.500 -29.056, 11.417 7.500 -29.000, 11.500 7.500 -27.500, 11.000 7.500 -29.000, 11.000 7.500 -25.000, 11.714 -7.000 -29.700, 12.000 -7.000 -30.000, 11.841 -7.000 -30.000, 11.417 -7.000 -29.000, 11.500 -7.000 -27.500, 11.417 -9.500 -29.000, 11.500 -9.500 -27.500, 11.000 -9.500 -25.000, 16.000 10.000 -61.000, 7.000 10.000 -46.766, 16.000 10.000 -63.531, -10.954 10.000 -30.246, 11.841 10.000 -30.000, 11.000 10.000 -30.000, -11.000 -8.500 -30.000, -11.000 -8.250 -29.500, -11.000 7.500 -30.000, -11.000 7.500 -29.000, -11.000 8.500 -30.000, -11.000 8.750 -29.500, -3.864 -12.000 1.035, -3.464 -12.000 2.000, -11.000 -12.000 10.000, -4.000 -12.000 0.000, -3.864 -12.000 -1.035, -11.000 -12.000 -29.000, 0.000 -12.000 -19.250, 11.000 -12.000 -29.000, 0.000 -12.000 -9.500, -3.464 -12.000 -2.000, -2.828 -12.000 -2.828, -2.000 -12.000 -3.464, -1.035 -12.000 -3.864, 0.000 -12.000 -4.000, 1.035 -12.000 -3.864, 2.000 -12.000 -3.464, 2.828 -12.000 -2.828, 3.464 -12.000 -2.000, 3.864 -12.000 -1.035, 11.000 -12.000 10.000, 4.000 -12.000 -0.000, 3.864 -12.000 1.035, 3.464 -12.000 2.000, 2.828 -12.000 2.828, 2.000 -12.000 3.464, 1.035 -12.000 3.864, 0.000 -12.000 4.000, 0.000 -12.000 7.000, -1.035 -12.000 3.864, -2.000 -12.000 3.464, 4.376 12.500 -6.697, 11.000 12.500 -29.000, 3.612 12.500 -7.138, 5.086 12.500 -6.175, 5.734 12.500 -5.578, 6.313 12.500 -4.914, 6.815 12.500 -4.189, 7.235 12.500 -3.414, 7.567 12.500 -2.598, -6.033 12.500 -5.254, -5.418 12.500 -5.886, -11.000 12.500 -29.000, -4.738 12.500 -6.446, -6.574 12.500 -4.558, -4.000 12.500 -6.928, -7.036 12.500 -3.808, -3.214 12.500 -7.326, -7.412 12.500 -3.010, -7.698 12.500 -2.177, -7.891 12.500 -1.317, -11.000 12.500 10.000, -6.574 12.500 4.558, -7.036 12.500 3.808, -7.412 12.500 3.010, -7.698 12.500 2.177, -7.891 12.500 1.317, -7.988 12.500 0.441, -7.988 12.500 -0.441, 11.000 0.250 -9.500, 11.000 10.000 -25.000, 11.000 -7.000 -25.000, 11.000 -7.000 -29.000, 11.000 0.250 0.250, 11.000 12.500 10.000, 11.417 10.000 -29.000, 11.714 10.000 -29.700, 11.506 10.000 -29.863, 11.181 10.000 -29.431, 10.857 10.000 -29.500, 10.000 10.000 -29.000, 11.000 10.000 -29.000, -0.000 11.250 -29.000, -11.000 10.000 -29.000, -11.000 0.250 -9.500, -11.000 6.375 -9.500, -11.000 0.250 0.250, -11.000 -5.875 -9.500, -11.000 -7.000 -29.000, -0.000 10.000 -29.500, 10.034 10.000 -29.259, 10.134 10.000 -29.500, 10.293 10.000 -29.707, 10.500 10.000 -29.866, 10.741 10.000 -29.966, -11.000 10.000 -30.000, 11.262 10.000 -29.965, -6.033 12.500 5.254, -5.418 12.500 5.886, -4.738 12.500 6.446, -4.000 12.500 6.928, -3.214 12.500 7.326, -2.388 12.500 7.635, -1.534 12.500 7.852, -0.661 12.500 7.973, 0.220 12.500 7.997, 8.000 12.500 0.000, 7.951 12.500 0.880, 7.806 12.500 1.749, 7.567 12.500 2.598, 7.235 12.500 3.414, 6.815 12.500 4.189, 6.313 12.500 4.914, 5.734 12.500 5.578, 5.086 12.500 6.175, 4.376 12.500 6.697, 3.612 12.500 7.138, 2.805 12.500 7.492, 1.964 12.500 7.755, 1.099 12.500 7.924, 7.951 12.500 -0.880, 7.806 12.500 -1.749, -0.000 12.500 -19.250, -0.000 12.500 -9.500, -2.388 12.500 -7.635, -1.534 12.500 -7.852, -0.661 12.500 -7.973, 0.220 12.500 -7.997, 1.099 12.500 -7.924, 1.964 12.500 -7.755, 2.805 12.500 -7.492, -2.828 -12.000 2.828, 10.000 -9.500 -29.000, 0.000 -10.750 -29.000, 10.000 -7.000 -29.000, -5.500 0.250 -29.000, -0.000 0.250 -29.000, -0.500 -3.375 -29.000, -0.500 3.875 -29.000, 5.000 0.250 -29.000, -11.000 10.000 -30.283, 11.673 10.000 -29.850, 12.000 10.000 -30.000, 11.500 10.000 -27.500, 12.000 10.000 -25.000, 12.000 -9.500 -25.000, 12.000 -9.500 -30.000, 11.714 -9.500 -29.700, 12.000 -7.000 -25.000, 12.000 7.500 -25.000, 12.000 7.500 -30.000, 11.714 7.500 -29.700, 11.841 7.500 -30.000, 7.806 13.500 1.749, 5.734 13.500 5.578, 5.086 13.500 6.175, 4.376 13.500 6.697, 3.612 13.500 7.138, 2.805 13.500 7.492, 1.964 13.500 7.755, 1.099 13.500 7.924, 0.220 13.500 7.997, -0.661 13.500 7.973, -1.534 13.500 7.852, -2.388 13.500 7.635, -3.214 13.500 7.326, -4.000 13.500 6.928, -4.738 13.500 6.446, -5.418 13.500 5.886, -6.033 13.500 5.254, -6.574 13.500 4.558, -7.036 13.500 3.808, -7.412 13.500 3.010, -7.698 13.500 2.177, -7.891 13.500 1.317, -7.988 13.500 0.441, -7.988 13.500 -0.441, -7.891 13.500 -1.317, -7.698 13.500 -2.177, -7.412 13.500 -3.010, -7.036 13.500 -3.808, -6.574 13.500 -4.558, -6.033 13.500 -5.254, -5.418 13.500 -5.886, -4.738 13.500 -6.446, -4.000 13.500 -6.928, -3.214 13.500 -7.326, -2.388 13.500 -7.635, -1.534 13.500 -7.852, -0.661 13.500 -7.973, 0.220 13.500 -7.997, 1.099 13.500 -7.924, 1.964 13.500 -7.755, 2.805 13.500 -7.492, 3.612 13.500 -7.138, 4.376 13.500 -6.697, 5.086 13.500 -6.175, 7.567 13.500 -2.598, 7.806 13.500 -1.749, 7.951 13.500 -0.880, -0.448 -12.500 3.975, 0.000 -12.500 4.000, -0.890 -12.500 3.900, -1.321 -12.500 3.776, -1.736 -12.500 3.604, -2.128 -12.500 3.387, -2.494 -12.500 3.127, -2.828 -12.500 2.828, -3.127 -12.500 2.494, -3.387 -12.500 2.128, -3.604 -12.500 1.736, -3.776 -12.500 1.321, -3.900 -12.500 0.890, -3.975 -12.500 0.448, -4.000 -12.500 0.000, -3.975 -12.500 -0.448, -3.900 -12.500 -0.890, -3.776 -12.500 -1.321, -3.604 -12.500 -1.736, -3.387 -12.500 -2.128, -3.127 -12.500 -2.494, -2.828 -12.500 -2.828, -2.494 -12.500 -3.127, -2.128 -12.500 -3.387, -1.736 -12.500 -3.604, -1.321 -12.500 -3.776, -0.890 -12.500 -3.900, -0.448 -12.500 -3.975, 0.000 -12.500 -4.000, 0.448 -12.500 -3.975, 0.890 -12.500 -3.900, 1.321 -12.500 -3.776, 1.736 -12.500 -3.604, 2.128 -12.500 -3.387, 2.494 -12.500 -3.127, 2.828 -12.500 -2.828, 3.127 -12.500 -2.494, 3.387 -12.500 -2.128, 3.604 -12.500 -1.736, 3.776 -12.500 -1.321, 3.900 -12.500 -0.890, 3.975 -12.500 -0.448, 4.000 -12.500 -0.000, 3.975 -12.500 0.448, 3.900 -12.500 0.890, 3.776 -12.500 1.321, 3.604 -12.500 1.736, 3.387 -12.500 2.128, 3.127 -12.500 2.494, 2.828 -12.500 2.828, 2.494 -12.500 3.127, 2.128 -12.500 3.387, 1.736 -12.500 3.604, 1.321 -12.500 3.776, 0.890 -12.500 3.900, 0.448 -12.500 3.975, -11.000 7.500 -30.283, -11.000 8.750 -30.141, 11.181 -9.500 -29.431, -9.834 -7.000 -29.056, 0.000 -7.000 -29.500, 10.034 -7.000 -29.259, 10.134 -7.000 -29.500, 10.293 -7.000 -29.707, 10.500 -7.000 -29.866, 10.741 -7.000 -29.966, 11.000 -7.000 -30.000, 11.506 -7.000 -29.863, 10.857 -7.000 -29.500, 11.262 -7.000 -29.965, 11.181 -7.000 -29.431, 11.181 7.500 -29.431, -11.000 -9.500 -30.283, -11.000 -8.250 -30.141, -11.000 -7.000 -30.283, 19.149 10.000 -63.038, 16.080 10.000 -63.629, 22.154 10.000 -62.175, 16.000 7.500 -63.531, -10.954 7.500 -30.246, 25.000 7.500 -61.000, 11.673 -9.500 -29.850, 11.673 -7.000 -29.850, 11.673 7.500 -29.850, -0.707 13.500 -0.707, -0.866 13.500 -0.500, -0.500 13.500 -0.866, -0.966 13.500 -0.259, -0.259 13.500 -0.966, -1.000 13.500 -0.000, -0.000 13.500 -1.000, -0.966 13.500 0.259, 0.259 13.500 -0.966, -0.866 13.500 0.500, 0.500 13.500 -0.866, -0.707 13.500 0.707, -0.500 13.500 0.866, -0.259 13.500 0.966, -0.000 13.500 1.000, 0.259 13.500 0.966, 0.500 13.500 0.866, 0.749 -12.500 -0.663, 0.663 -12.500 -0.749, 0.568 -12.500 -0.823, 0.823 -12.500 -0.568, 0.885 -12.500 -0.465, 0.465 -12.500 -0.885, 0.935 -12.500 -0.355, 0.355 -12.500 -0.935, 0.971 -12.500 -0.239, 0.239 -12.500 -0.971, 0.993 -12.500 -0.121, 0.121 -12.500 -0.993, 0.000 -12.500 -1.000, 1.000 -12.500 0.000, -0.121 -12.500 -0.993, 0.993 -12.500 0.121, -0.239 -12.500 -0.971, 0.971 -12.500 0.239, -0.355 -12.500 -0.935, 0.935 -12.500 0.355, -0.465 -12.500 -0.885, 0.885 -12.500 0.465, -0.568 -12.500 -0.823, 0.823 -12.500 0.568, -0.663 -12.500 -0.749, 0.749 -12.500 0.663, -0.749 -12.500 -0.663, 0.663 -12.500 0.749, 0.568 -12.500 0.823, -0.823 -12.500 -0.568, 0.465 -12.500 0.885, -0.885 -12.500 -0.465, 0.355 -12.500 0.935, -0.935 -12.500 -0.355, -0.971 -12.500 -0.239, 0.239 -12.500 0.971, 0.121 -12.500 0.993, -0.993 -12.500 -0.121, 0.000 -12.500 1.000, -1.000 -12.500 -0.000, -0.993 -12.500 0.121, -0.121 -12.500 0.993, -0.239 -12.500 0.971, -0.971 -12.500 0.239, -0.935 -12.500 0.355, -0.355 -12.500 0.935, -0.885 -12.500 0.465, -0.465 -12.500 0.885, -0.568 -12.500 0.823, -0.823 -12.500 0.568, -0.663 -12.500 0.749, -0.749 -12.500 0.663, 2.500 7.500 -46.766, 16.000 7.500 -63.500, 16.033 7.500 -61.000, 17.125 7.500 -62.348, 17.125 -7.000 -62.348, 16.000 10.000 -63.598, 28.906 10.000 -68.674, 28.605 10.000 -68.434, 29.247 10.000 -68.853, 28.296 10.000 -65.953, 28.127 10.000 -66.299, 28.027 10.000 -66.671, 28.001 10.000 -67.055, 28.048 10.000 -67.437, 28.168 10.000 -67.803, 28.356 10.000 -68.140, 29.617 10.000 -68.963, 30.000 10.000 -69.000, 16.000 7.500 -63.598, 25.177 9.000 -61.000, 25.089 8.750 -61.000, 25.177 8.000 -61.000, 25.177 7.500 -61.000, -0.121 14.500 -0.993, -0.000 14.500 -1.000, -0.239 14.500 -0.971, -0.355 14.500 -0.935, -0.465 14.500 -0.885, -0.568 14.500 -0.823, -0.663 14.500 -0.749, -0.749 14.500 -0.663, -0.823 14.500 -0.568, -0.885 14.500 -0.465, -0.935 14.500 -0.355, -0.971 14.500 -0.239, -0.993 14.500 -0.121, -1.000 14.500 -0.000, -0.993 14.500 0.121, -0.971 14.500 0.239, -0.935 14.500 0.355, -0.885 14.500 0.465, -0.823 14.500 0.568, -0.749 14.500 0.663, -0.663 14.500 0.749, -0.568 14.500 0.823, -0.465 14.500 0.885, -0.355 14.500 0.935, -0.239 14.500 0.971, -0.121 14.500 0.993, -0.000 14.500 1.000, 0.121 14.500 0.993, 0.239 14.500 0.971, 0.355 14.500 0.935, 0.465 14.500 0.885, 0.568 14.500 0.823, 0.663 14.500 0.749, 0.749 14.500 0.663, 0.823 14.500 0.568, 0.885 14.500 0.465, 0.935 14.500 0.355, 0.971 14.500 0.239, 0.993 14.500 0.121, 1.000 14.500 0.000, 0.993 14.500 -0.121, 0.971 14.500 -0.239, 0.935 14.500 -0.355, 0.885 14.500 -0.465, 0.823 14.500 -0.568, 0.749 14.500 -0.663, 0.663 14.500 -0.749, 0.568 14.500 -0.823, 0.465 14.500 -0.885, 0.355 14.500 -0.935, 0.239 14.500 -0.971, 0.121 14.500 -0.993, -0.121 -13.500 -0.993, 0.000 -13.500 -1.000, -0.239 -13.500 -0.971, -0.355 -13.500 -0.935, -0.465 -13.500 -0.885, -0.568 -13.500 -0.823, -0.663 -13.500 -0.749, -0.749 -13.500 -0.663, -0.823 -13.500 -0.568, -0.885 -13.500 -0.465, -0.935 -13.500 -0.355, -0.971 -13.500 -0.239, -0.993 -13.500 -0.121, -1.000 -13.500 -0.000, -0.993 -13.500 0.121, -0.971 -13.500 0.239, -0.935 -13.500 0.355, -0.885 -13.500 0.465, -0.823 -13.500 0.568, -0.749 -13.500 0.663, -0.663 -13.500 0.749, -0.568 -13.500 0.823, -0.465 -13.500 0.885, -0.355 -13.500 0.935, -0.239 -13.500 0.971, -0.121 -13.500 0.993, 0.000 -13.500 1.000, 0.121 -13.500 0.993, 0.239 -13.500 0.971, 0.355 -13.500 0.935, 0.465 -13.500 0.885, 0.568 -13.500 0.823, 0.663 -13.500 0.749, 0.749 -13.500 0.663, 0.823 -13.500 0.568, 0.885 -13.500 0.465, 0.935 -13.500 0.355, 0.971 -13.500 0.239, 0.993 -13.500 0.121, 1.000 -13.500 0.000, 0.993 -13.500 -0.121, 0.971 -13.500 -0.239, 0.935 -13.500 -0.355, 0.885 -13.500 -0.465, 0.823 -13.500 -0.568, 0.749 -13.500 -0.663, 0.663 -13.500 -0.749, 0.568 -13.500 -0.823, 0.465 -13.500 -0.885, 0.355 -13.500 -0.935, 0.239 -13.500 -0.971, 0.121 -13.500 -0.993, 16.123 7.500 -63.565, 16.246 7.500 -63.605, 16.080 7.500 -63.629, 16.135 7.500 -63.621, 16.191 7.500 -63.613, 18.893 -7.000 -63.098, 20.993 -7.000 -62.543, 20.998 0.250 -63.992, 22.934 0.250 -64.814, 18.893 7.500 -63.098, 19.061 0.250 -63.170, 20.552 7.500 -62.049, 20.993 7.500 -62.543, 23.055 7.500 -61.858, 25.070 7.500 -61.047, 19.149 -9.500 -63.038, 22.154 -9.500 -62.175, 16.123 -7.000 -63.565, 16.246 -7.000 -63.605, 16.080 -7.000 -63.629, 16.135 -7.000 -63.621, 16.191 -7.000 -63.613, 20.790 7.500 -65.533, 30.000 7.500 -69.000, 31.000 7.500 -65.047, 31.000 7.500 -65.268, 28.035 8.750 -63.047, 25.070 8.000 -61.047, 25.070 9.000 -61.047, 25.106 9.000 -61.032, 25.142 9.000 -61.016, 18.395 7.500 -64.532, 25.106 8.000 -61.032, 25.142 8.000 -61.016, 25.106 7.500 -61.032, 25.142 7.500 -61.016, 25.089 7.500 -61.024, 25.089 -8.250 -61.000, 25.177 -8.000 -61.000, 25.177 -9.000 -61.000, -0.000 14.500 0.000, 0.000 -13.500 0.000, 24.285 7.500 -67.017, 22.213 -0.500 -66.137, 24.310 -8.500 -66.956, 24.578 -0.500 -66.326, 28.906 7.500 -68.674, 28.605 7.500 -68.434, 28.356 7.500 -68.140, 28.168 7.500 -67.803, 29.247 7.500 -68.853, 30.649 7.500 -65.108, 28.048 7.500 -67.437, 30.275 7.500 -65.019, 29.146 7.500 -65.191, 29.509 7.500 -65.061, 29.890 7.500 -65.003, 29.617 7.500 -68.963, 28.001 7.500 -67.055, 28.027 7.500 -66.671, 16.000 -9.500 -63.598, 29.371 -9.500 -69.000, 28.906 -9.500 -68.674, 29.247 -9.500 -68.853, 31.000 -9.500 -65.268, 31.000 -9.500 -65.047, 30.649 -9.500 -65.108, 30.275 -9.500 -65.019, 29.146 -9.500 -65.191, 28.816 -9.500 -65.388, 29.509 -9.500 -65.061, 29.890 -9.500 -65.003, 29.617 -9.500 -68.963, 30.000 -9.500 -69.000, 16.000 -7.000 -63.598, 18.395 -7.000 -64.532, 30.390 10.000 -68.962, 30.390 7.500 -68.962, 30.765 10.000 -68.848, 30.765 7.500 -68.848, 31.111 10.000 -68.663, 31.111 7.500 -68.663, 31.414 10.000 -68.414, 31.414 7.500 -68.414, 31.663 10.000 -68.111, 31.663 7.500 -68.111, 31.848 10.000 -67.765, 31.848 7.500 -67.765, 31.962 10.000 -67.390, 31.962 7.500 -67.390, 32.000 7.500 -67.000, 32.000 10.000 -67.000, 31.956 10.000 -66.584, 31.827 10.000 -66.187, 31.618 10.000 -65.824, 31.338 10.000 -65.514, 31.956 7.500 -66.584, 31.827 7.500 -66.187, 31.618 7.500 -65.824, 31.338 7.500 -65.514, 25.070 -8.000 -61.047, 25.106 -8.000 -61.032, 25.142 -8.000 -61.016, 25.106 -9.000 -61.032, 25.070 -9.000 -61.047, 25.142 -9.000 -61.016, 28.035 -8.250 -63.047, 29.500 7.500 -67.002, 31.000 7.500 -68.000, 30.000 7.500 -68.000, 29.500 -9.500 -67.002, 31.000 -9.500 -68.000, 30.000 -9.500 -68.000, 30.390 -7.000 -68.962, 30.390 -9.500 -68.962, 30.765 -7.000 -68.848, 30.765 -9.500 -68.848, 31.111 -7.000 -68.663, 31.111 -9.500 -68.663, 31.414 -7.000 -68.414, 31.414 -9.500 -68.414, 31.663 -7.000 -68.111, 31.663 -9.500 -68.111, 31.848 -7.000 -67.765, 31.848 -9.500 -67.765, 31.962 -7.000 -67.390, 31.962 -9.500 -67.390, 32.000 -9.500 -67.000, 32.000 -7.000 -67.000, 31.956 -7.000 -66.584, 31.827 -7.000 -66.187, 31.618 -7.000 -65.824, 31.338 -7.000 -65.514, 31.956 -9.500 -66.584, 31.827 -9.500 -66.187, 31.618 -9.500 -65.824, 31.338 -9.500 -65.514 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 1, 3, 2, -1, 0, 2, 4, -1, 5, 6, 7, -1, 8, 6, 5, -1, 8, 9, 6, -1, 10, 11, 12, -1, 13, 10, 12, -1, 14, 13, 12, -1, 15, 14, 12, -1, 16, 15, 12, -1, 17, 16, 12, -1, 18, 17, 12, -1, 18, 12, 19, -1, 19, 12, 20, -1, 21, 22, 23, -1, 24, 23, 22, -1, 25, 24, 22, -1, 25, 22, 26, -1, 27, 28, 29, -1, 30, 27, 29, -1, 30, 29, 31, -1, 32, 33, 34, -1, 33, 32, 35, -1, 36, 34, 33, -1, 33, 35, 37, -1, 37, 35, 38, -1, 39, 36, 33, -1, 40, 37, 38, -1, 41, 39, 33, -1, 41, 33, 42, -1, 37, 40, 43, -1, 44, 41, 42, -1, 45, 44, 42, -1, 28, 46, 47, -1, 48, 46, 28, -1, 49, 48, 28, -1, 50, 49, 28, -1, 51, 50, 28, -1, 51, 0, 4, -1, 4, 50, 51, -1, 28, 47, 52, -1, 47, 53, 52, -1, 53, 54, 52, -1, 54, 45, 52, -1, 45, 42, 52, -1, 55, 56, 57, -1, 56, 55, 58, -1, 59, 60, 61, -1, 60, 62, 61, -1, 59, 61, 63, -1, 64, 65, 66, -1, 67, 65, 64, -1, 28, 68, 69, -1, 69, 68, 70, -1, 71, 72, 73, -1, 72, 74, 73, -1, 71, 73, 75, -1, 76, 77, 78, -1, 79, 76, 78, -1, 79, 78, 80, -1, 80, 78, 81, -1, 78, 77, 81, -1, 77, 82, 83, -1, 77, 83, 84, -1, 85, 82, 77, -1, 86, 85, 77, -1, 87, 86, 77, -1, 88, 87, 77, -1, 88, 59, 63, -1, 63, 87, 88, -1, 77, 84, 89, -1, 73, 90, 91, -1, 90, 73, 92, -1, 90, 92, 93, -1, 94, 90, 93, -1, 91, 90, 94, -1, 95, 96, 97, -1, 98, 97, 96, -1, 99, 98, 96, -1, 95, 100, 96, -1, 101, 102, 103, -1, 101, 103, 104, -1, 105, 106, 107, -1, 105, 108, 106, -1, 109, 110, 106, -1, 109, 111, 110, -1, 112, 113, 110, -1, 112, 114, 113, -1, 115, 116, 117, -1, 115, 118, 116, -1, 119, 120, 116, -1, 119, 121, 120, -1, 122, 123, 124, -1, 125, 123, 122, -1, 124, 123, 126, -1, 127, 128, 129, -1, 128, 130, 129, -1, 131, 132, 127, -1, 133, 132, 131, -1, 127, 132, 134, -1, 132, 135, 136, -1, 132, 136, 137, -1, 138, 132, 137, -1, 134, 132, 138, -1, 132, 139, 135, -1, 140, 141, 142, -1, 143, 140, 142, -1, 144, 143, 142, -1, 144, 142, 145, -1, 145, 142, 126, -1, 126, 142, 124, -1, 142, 146, 104, -1, 124, 142, 104, -1, 146, 142, 141, -1, 147, 148, 149, -1, 149, 148, 150, -1, 151, 152, 153, -1, 152, 151, 154, -1, 152, 154, 155, -1, 156, 157, 139, -1, 139, 157, 158, -1, 81, 159, 160, -1, 160, 159, 161, -1, 162, 160, 161, -1, 163, 81, 160, -1, 163, 160, 164, -1, 164, 160, 162, -1, 129, 130, 165, -1, 130, 166, 165, -1, 167, 168, 169, -1, 168, 170, 169, -1, 171, 172, 173, -1, 174, 171, 173, -1, 175, 174, 173, -1, 176, 175, 173, -1, 177, 178, 179, -1, 177, 176, 178, -1, 177, 179, 176, -1, 176, 179, 180, -1, 176, 180, 175, -1, 180, 179, 181, -1, 179, 182, 181, -1, 179, 183, 182, -1, 179, 184, 183, -1, 179, 185, 184, -1, 179, 186, 185, -1, 186, 179, 187, -1, 187, 179, 188, -1, 178, 188, 179, -1, 189, 190, 191, -1, 191, 190, 192, -1, 192, 190, 193, -1, 193, 190, 194, -1, 190, 195, 194, -1, 190, 196, 195, -1, 190, 197, 196, -1, 198, 197, 190, -1, 198, 173, 197, -1, 198, 190, 173, -1, 199, 173, 200, -1, 197, 173, 199, -1, 201, 202, 203, -1, 204, 202, 201, -1, 205, 202, 204, -1, 206, 202, 205, -1, 202, 206, 207, -1, 202, 207, 208, -1, 202, 208, 209, -1, 210, 211, 212, -1, 213, 212, 211, -1, 214, 210, 212, -1, 215, 212, 213, -1, 216, 214, 212, -1, 217, 212, 215, -1, 218, 216, 212, -1, 219, 218, 212, -1, 220, 219, 212, -1, 221, 220, 212, -1, 221, 222, 223, -1, 221, 223, 224, -1, 221, 224, 225, -1, 221, 225, 226, -1, 221, 226, 227, -1, 221, 227, 228, -1, 228, 220, 221, -1, 229, 150, 230, -1, 229, 231, 150, -1, 231, 232, 150, -1, 232, 149, 150, -1, 229, 158, 231, -1, 190, 158, 229, -1, 158, 178, 139, -1, 158, 190, 178, -1, 233, 190, 229, -1, 233, 234, 190, -1, 233, 229, 234, -1, 234, 229, 230, -1, 235, 236, 237, -1, 238, 235, 237, -1, 238, 239, 235, -1, 238, 237, 239, -1, 240, 202, 241, -1, 242, 202, 240, -1, 243, 212, 242, -1, 240, 243, 242, -1, 212, 202, 242, -1, 243, 244, 212, -1, 245, 221, 212, -1, 245, 244, 221, -1, 245, 212, 244, -1, 246, 173, 221, -1, 246, 244, 173, -1, 246, 221, 244, -1, 247, 176, 173, -1, 247, 244, 176, -1, 247, 173, 244, -1, 168, 244, 243, -1, 176, 244, 130, -1, 248, 244, 168, -1, 130, 244, 248, -1, 243, 240, 249, -1, 240, 250, 249, -1, 250, 251, 249, -1, 251, 252, 249, -1, 252, 253, 249, -1, 253, 254, 249, -1, 254, 164, 249, -1, 164, 255, 249, -1, 249, 255, 243, -1, 241, 250, 240, -1, 239, 251, 250, -1, 239, 252, 251, -1, 239, 253, 252, -1, 239, 254, 253, -1, 239, 164, 254, -1, 239, 256, 164, -1, 237, 256, 239, -1, 241, 239, 250, -1, 235, 239, 241, -1, 230, 241, 202, -1, 234, 230, 202, -1, 257, 222, 221, -1, 258, 257, 221, -1, 259, 258, 221, -1, 260, 259, 221, -1, 261, 260, 221, -1, 262, 261, 221, -1, 263, 262, 221, -1, 263, 221, 264, -1, 264, 221, 265, -1, 265, 221, 234, -1, 266, 267, 234, -1, 267, 268, 234, -1, 268, 269, 234, -1, 234, 269, 270, -1, 234, 270, 271, -1, 234, 271, 272, -1, 234, 272, 273, -1, 234, 273, 274, -1, 234, 274, 275, -1, 234, 275, 276, -1, 234, 276, 277, -1, 277, 278, 234, -1, 278, 279, 234, -1, 279, 265, 234, -1, 202, 266, 234, -1, 202, 280, 266, -1, 202, 281, 280, -1, 202, 209, 281, -1, 282, 202, 212, -1, 282, 283, 202, -1, 282, 212, 283, -1, 203, 202, 283, -1, 217, 284, 283, -1, 285, 283, 284, -1, 286, 283, 285, -1, 287, 283, 286, -1, 288, 283, 287, -1, 289, 283, 288, -1, 289, 290, 283, -1, 290, 203, 283, -1, 283, 212, 217, -1, 221, 173, 190, -1, 221, 190, 234, -1, 178, 190, 189, -1, 178, 189, 188, -1, 172, 291, 173, -1, 200, 173, 291, -1, 170, 243, 255, -1, 169, 170, 255, -1, 168, 243, 170, -1, 178, 292, 139, -1, 176, 130, 293, -1, 178, 176, 293, -1, 130, 292, 293, -1, 293, 292, 178, -1, 232, 141, 149, -1, 294, 141, 232, -1, 295, 248, 168, -1, 295, 296, 248, -1, 295, 168, 296, -1, 297, 294, 248, -1, 297, 296, 294, -1, 297, 248, 296, -1, 298, 168, 141, -1, 298, 296, 168, -1, 298, 141, 296, -1, 299, 141, 294, -1, 299, 296, 141, -1, 299, 294, 296, -1, 130, 248, 166, -1, 166, 248, 91, -1, 165, 166, 91, -1, 164, 162, 255, -1, 255, 162, 300, -1, 163, 256, 237, -1, 301, 163, 237, -1, 301, 236, 163, -1, 301, 237, 236, -1, 256, 163, 164, -1, 236, 302, 163, -1, 302, 236, 235, -1, 230, 303, 241, -1, 304, 302, 303, -1, 304, 303, 230, -1, 303, 235, 241, -1, 302, 235, 303, -1, 305, 157, 306, -1, 157, 305, 158, -1, 306, 157, 156, -1, 306, 307, 100, -1, 306, 156, 307, -1, 231, 158, 308, -1, 308, 158, 305, -1, 308, 155, 231, -1, 308, 152, 155, -1, 231, 155, 232, -1, 155, 154, 232, -1, 309, 148, 310, -1, 148, 309, 150, -1, 310, 148, 147, -1, 310, 311, 312, -1, 310, 147, 311, -1, 230, 150, 304, -1, 304, 150, 309, -1, 112, 267, 266, -1, 266, 114, 112, -1, 268, 267, 112, -1, 268, 112, 313, -1, 269, 268, 313, -1, 269, 313, 111, -1, 270, 269, 111, -1, 111, 109, 270, -1, 271, 270, 109, -1, 109, 108, 271, -1, 272, 271, 108, -1, 272, 108, 105, -1, 273, 272, 105, -1, 273, 105, 314, -1, 274, 273, 314, -1, 314, 315, 274, -1, 315, 275, 274, -1, 315, 316, 275, -1, 276, 275, 316, -1, 276, 316, 317, -1, 277, 276, 317, -1, 277, 317, 318, -1, 318, 278, 277, -1, 318, 319, 278, -1, 319, 279, 278, -1, 319, 320, 279, -1, 321, 265, 279, -1, 279, 320, 321, -1, 321, 264, 265, -1, 264, 321, 322, -1, 323, 263, 264, -1, 264, 322, 323, -1, 323, 262, 263, -1, 262, 323, 324, -1, 325, 261, 262, -1, 324, 325, 262, -1, 260, 261, 325, -1, 260, 325, 326, -1, 327, 259, 260, -1, 260, 326, 327, -1, 258, 259, 327, -1, 258, 327, 328, -1, 328, 329, 258, -1, 257, 258, 329, -1, 222, 257, 329, -1, 222, 329, 330, -1, 223, 222, 330, -1, 223, 330, 331, -1, 224, 223, 331, -1, 224, 331, 332, -1, 225, 224, 332, -1, 332, 333, 225, -1, 225, 333, 334, -1, 226, 225, 334, -1, 227, 226, 334, -1, 334, 335, 227, -1, 228, 227, 335, -1, 228, 335, 336, -1, 220, 228, 336, -1, 336, 337, 220, -1, 219, 220, 337, -1, 337, 338, 219, -1, 339, 218, 219, -1, 338, 339, 219, -1, 216, 218, 339, -1, 216, 339, 340, -1, 340, 214, 216, -1, 214, 340, 341, -1, 341, 210, 214, -1, 210, 341, 342, -1, 211, 210, 343, -1, 210, 342, 343, -1, 213, 211, 343, -1, 343, 344, 213, -1, 344, 215, 213, -1, 344, 345, 215, -1, 217, 215, 346, -1, 215, 345, 346, -1, 284, 217, 346, -1, 346, 347, 284, -1, 347, 285, 284, -1, 285, 347, 348, -1, 348, 286, 285, -1, 348, 349, 286, -1, 287, 286, 350, -1, 286, 349, 350, -1, 288, 287, 350, -1, 350, 351, 288, -1, 351, 289, 288, -1, 351, 352, 289, -1, 352, 290, 289, -1, 352, 353, 290, -1, 353, 203, 290, -1, 353, 354, 203, -1, 354, 201, 203, -1, 201, 354, 355, -1, 204, 201, 355, -1, 355, 356, 204, -1, 356, 205, 204, -1, 356, 121, 205, -1, 206, 205, 121, -1, 206, 121, 119, -1, 207, 206, 119, -1, 119, 118, 207, -1, 208, 207, 118, -1, 118, 115, 208, -1, 208, 115, 357, -1, 209, 208, 357, -1, 281, 209, 357, -1, 281, 357, 358, -1, 358, 280, 281, -1, 280, 358, 359, -1, 359, 266, 280, -1, 266, 359, 114, -1, 197, 199, 360, -1, 197, 360, 361, -1, 362, 360, 199, -1, 199, 363, 362, -1, 199, 364, 363, -1, 364, 199, 200, -1, 200, 365, 364, -1, 200, 366, 365, -1, 200, 291, 366, -1, 367, 366, 291, -1, 291, 368, 367, -1, 172, 368, 291, -1, 172, 369, 368, -1, 370, 369, 172, -1, 172, 371, 370, -1, 371, 172, 171, -1, 171, 372, 371, -1, 373, 372, 171, -1, 174, 373, 171, -1, 374, 373, 174, -1, 375, 374, 174, -1, 175, 375, 174, -1, 376, 375, 175, -1, 175, 377, 376, -1, 175, 180, 377, -1, 378, 377, 180, -1, 379, 378, 180, -1, 380, 379, 180, -1, 380, 180, 181, -1, 181, 381, 380, -1, 382, 381, 181, -1, 382, 181, 182, -1, 383, 382, 182, -1, 182, 384, 383, -1, 182, 183, 384, -1, 384, 183, 385, -1, 386, 385, 183, -1, 183, 387, 386, -1, 183, 184, 387, -1, 184, 388, 387, -1, 184, 389, 388, -1, 184, 185, 389, -1, 390, 389, 185, -1, 185, 391, 390, -1, 185, 392, 391, -1, 392, 185, 186, -1, 186, 393, 392, -1, 186, 394, 393, -1, 186, 187, 394, -1, 395, 394, 187, -1, 187, 396, 395, -1, 188, 396, 187, -1, 188, 397, 396, -1, 398, 397, 188, -1, 188, 399, 398, -1, 399, 188, 189, -1, 189, 400, 399, -1, 401, 400, 189, -1, 401, 189, 191, -1, 402, 401, 191, -1, 191, 403, 402, -1, 191, 192, 403, -1, 404, 403, 192, -1, 192, 405, 404, -1, 192, 193, 405, -1, 406, 405, 193, -1, 407, 406, 193, -1, 408, 407, 193, -1, 408, 193, 194, -1, 194, 409, 408, -1, 410, 409, 194, -1, 410, 194, 195, -1, 411, 410, 195, -1, 195, 412, 411, -1, 412, 195, 413, -1, 195, 196, 413, -1, 414, 413, 196, -1, 196, 415, 414, -1, 196, 197, 415, -1, 361, 415, 197, -1, 416, 167, 169, -1, 169, 255, 417, -1, 417, 255, 300, -1, 300, 416, 417, -1, 416, 169, 417, -1, 146, 168, 167, -1, 141, 168, 146, -1, 139, 292, 135, -1, 307, 156, 133, -1, 156, 139, 132, -1, 418, 156, 132, -1, 418, 133, 156, -1, 418, 132, 133, -1, 135, 292, 128, -1, 136, 135, 128, -1, 137, 136, 128, -1, 137, 128, 138, -1, 138, 128, 134, -1, 134, 128, 127, -1, 130, 128, 292, -1, 419, 91, 248, -1, 294, 419, 248, -1, 419, 294, 420, -1, 294, 421, 420, -1, 421, 422, 420, -1, 422, 423, 420, -1, 423, 424, 420, -1, 424, 425, 420, -1, 425, 426, 420, -1, 420, 101, 419, -1, 426, 101, 420, -1, 232, 421, 294, -1, 154, 151, 427, -1, 428, 422, 421, -1, 428, 423, 422, -1, 428, 424, 423, -1, 428, 425, 424, -1, 428, 426, 425, -1, 428, 429, 426, -1, 427, 429, 428, -1, 154, 428, 232, -1, 232, 428, 421, -1, 430, 154, 427, -1, 430, 428, 154, -1, 430, 427, 428, -1, 149, 141, 140, -1, 311, 147, 125, -1, 123, 140, 143, -1, 123, 143, 144, -1, 145, 123, 144, -1, 126, 123, 145, -1, 147, 149, 123, -1, 123, 149, 140, -1, 431, 147, 123, -1, 431, 125, 147, -1, 431, 123, 125, -1, 432, 129, 165, -1, 165, 91, 433, -1, 433, 91, 434, -1, 434, 432, 433, -1, 432, 165, 433, -1, 435, 161, 159, -1, 435, 436, 161, -1, 437, 435, 159, -1, 437, 159, 81, -1, 77, 437, 81, -1, 438, 162, 161, -1, 438, 439, 162, -1, 162, 439, 416, -1, 300, 162, 416, -1, 312, 81, 163, -1, 312, 440, 81, -1, 304, 310, 302, -1, 309, 310, 304, -1, 312, 163, 310, -1, 310, 163, 302, -1, 100, 153, 306, -1, 306, 153, 152, -1, 100, 133, 131, -1, 441, 100, 307, -1, 441, 133, 100, -1, 441, 307, 133, -1, 127, 100, 131, -1, 308, 306, 152, -1, 305, 306, 308, -1, 153, 429, 427, -1, 442, 153, 427, -1, 442, 151, 153, -1, 442, 427, 151, -1, 429, 153, 426, -1, 312, 125, 122, -1, 443, 312, 311, -1, 443, 125, 312, -1, 443, 311, 125, -1, 124, 312, 122, -1, 444, 342, 341, -1, 444, 343, 342, -1, 444, 344, 343, -1, 445, 340, 339, -1, 445, 341, 340, -1, 445, 444, 341, -1, 446, 345, 344, -1, 446, 346, 345, -1, 446, 344, 444, -1, 447, 445, 339, -1, 447, 338, 337, -1, 447, 339, 338, -1, 448, 347, 346, -1, 448, 348, 347, -1, 448, 349, 348, -1, 448, 346, 446, -1, 449, 447, 337, -1, 449, 335, 334, -1, 449, 336, 335, -1, 449, 337, 336, -1, 450, 350, 349, -1, 450, 351, 350, -1, 450, 349, 448, -1, 451, 449, 334, -1, 451, 333, 332, -1, 451, 334, 333, -1, 452, 352, 351, -1, 452, 353, 352, -1, 452, 351, 450, -1, 453, 331, 330, -1, 453, 332, 331, -1, 453, 451, 332, -1, 454, 354, 353, -1, 454, 355, 354, -1, 454, 356, 355, -1, 454, 353, 452, -1, 455, 453, 330, -1, 455, 330, 329, -1, 120, 356, 454, -1, 121, 356, 120, -1, 328, 455, 329, -1, 327, 455, 328, -1, 327, 456, 455, -1, 118, 119, 116, -1, 326, 456, 327, -1, 325, 456, 326, -1, 325, 457, 456, -1, 357, 115, 117, -1, 324, 457, 325, -1, 358, 357, 117, -1, 323, 457, 324, -1, 359, 358, 117, -1, 359, 117, 113, -1, 322, 457, 323, -1, 322, 458, 457, -1, 114, 359, 113, -1, 321, 458, 322, -1, 320, 458, 321, -1, 320, 459, 458, -1, 313, 112, 110, -1, 319, 459, 320, -1, 111, 313, 110, -1, 318, 459, 319, -1, 318, 460, 459, -1, 317, 460, 318, -1, 108, 109, 106, -1, 316, 460, 317, -1, 315, 460, 316, -1, 315, 107, 460, -1, 314, 105, 107, -1, 314, 107, 315, -1, 395, 396, 461, -1, 395, 462, 394, -1, 395, 461, 462, -1, 394, 463, 393, -1, 394, 462, 463, -1, 396, 464, 461, -1, 396, 397, 464, -1, 397, 398, 465, -1, 397, 465, 464, -1, 393, 466, 392, -1, 393, 463, 466, -1, 398, 399, 467, -1, 398, 467, 465, -1, 392, 466, 468, -1, 392, 468, 391, -1, 399, 400, 469, -1, 399, 469, 467, -1, 391, 470, 390, -1, 391, 468, 470, -1, 400, 471, 469, -1, 400, 401, 471, -1, 390, 472, 389, -1, 390, 470, 472, -1, 388, 473, 387, -1, 389, 473, 388, -1, 389, 472, 473, -1, 401, 402, 474, -1, 402, 403, 474, -1, 401, 474, 471, -1, 387, 475, 386, -1, 387, 473, 475, -1, 403, 476, 474, -1, 403, 404, 476, -1, 386, 477, 385, -1, 386, 475, 477, -1, 404, 405, 478, -1, 404, 478, 476, -1, 385, 479, 384, -1, 385, 477, 479, -1, 405, 406, 480, -1, 405, 480, 478, -1, 384, 479, 481, -1, 384, 481, 383, -1, 406, 407, 482, -1, 406, 482, 480, -1, 383, 481, 483, -1, 383, 483, 382, -1, 407, 484, 482, -1, 407, 408, 484, -1, 382, 483, 485, -1, 408, 486, 484, -1, 408, 409, 486, -1, 382, 485, 381, -1, 485, 487, 381, -1, 486, 409, 488, -1, 409, 410, 488, -1, 488, 410, 489, -1, 381, 487, 380, -1, 487, 490, 380, -1, 489, 410, 411, -1, 489, 411, 491, -1, 490, 379, 380, -1, 490, 492, 379, -1, 491, 411, 412, -1, 491, 412, 493, -1, 492, 378, 379, -1, 492, 494, 378, -1, 494, 377, 378, -1, 494, 495, 377, -1, 493, 412, 413, -1, 493, 413, 496, -1, 496, 413, 414, -1, 496, 414, 497, -1, 495, 376, 377, -1, 495, 498, 376, -1, 497, 414, 415, -1, 497, 415, 499, -1, 498, 375, 376, -1, 498, 500, 375, -1, 500, 374, 375, -1, 499, 415, 361, -1, 500, 373, 374, -1, 500, 501, 373, -1, 499, 361, 360, -1, 499, 360, 502, -1, 502, 360, 362, -1, 502, 362, 503, -1, 501, 372, 373, -1, 501, 504, 372, -1, 504, 371, 372, -1, 504, 505, 371, -1, 503, 362, 363, -1, 503, 363, 506, -1, 505, 370, 371, -1, 505, 507, 370, -1, 506, 363, 364, -1, 506, 364, 508, -1, 508, 364, 365, -1, 508, 365, 509, -1, 507, 369, 370, -1, 507, 510, 369, -1, 509, 365, 366, -1, 509, 366, 511, -1, 510, 368, 369, -1, 510, 512, 368, -1, 511, 366, 367, -1, 512, 367, 368, -1, 511, 367, 512, -1, 439, 167, 416, -1, 439, 438, 513, -1, 71, 167, 513, -1, 514, 71, 513, -1, 438, 514, 513, -1, 167, 439, 513, -1, 101, 104, 146, -1, 146, 419, 101, -1, 104, 103, 515, -1, 515, 103, 102, -1, 516, 515, 102, -1, 102, 517, 516, -1, 515, 124, 104, -1, 440, 312, 515, -1, 515, 312, 124, -1, 146, 91, 419, -1, 91, 146, 167, -1, 99, 129, 432, -1, 127, 129, 99, -1, 96, 100, 127, -1, 96, 127, 99, -1, 426, 102, 101, -1, 153, 102, 426, -1, 153, 69, 102, -1, 91, 94, 434, -1, 94, 99, 432, -1, 434, 94, 432, -1, 161, 436, 518, -1, 518, 436, 65, -1, 436, 519, 65, -1, 436, 520, 519, -1, 65, 519, 521, -1, 89, 436, 435, -1, 437, 89, 435, -1, 77, 89, 437, -1, 520, 436, 89, -1, 84, 522, 89, -1, 522, 523, 89, -1, 523, 524, 89, -1, 524, 525, 89, -1, 525, 526, 89, -1, 526, 527, 89, -1, 527, 528, 89, -1, 528, 520, 89, -1, 529, 65, 521, -1, 65, 529, 530, -1, 531, 438, 161, -1, 531, 161, 518, -1, 81, 532, 80, -1, 440, 533, 81, -1, 81, 533, 532, -1, 532, 533, 534, -1, 534, 533, 440, -1, 534, 440, 535, -1, 69, 100, 95, -1, 100, 69, 153, -1, 536, 450, 448, -1, 536, 537, 450, -1, 538, 536, 448, -1, 448, 539, 538, -1, 448, 446, 539, -1, 446, 540, 539, -1, 541, 540, 446, -1, 446, 444, 541, -1, 444, 542, 541, -1, 444, 543, 542, -1, 444, 544, 543, -1, 544, 444, 445, -1, 545, 544, 445, -1, 445, 546, 545, -1, 445, 447, 546, -1, 447, 547, 546, -1, 447, 548, 547, -1, 447, 449, 548, -1, 549, 548, 449, -1, 449, 550, 549, -1, 449, 451, 550, -1, 451, 551, 550, -1, 552, 551, 451, -1, 552, 451, 453, -1, 553, 552, 453, -1, 453, 554, 553, -1, 453, 455, 554, -1, 555, 554, 455, -1, 455, 556, 555, -1, 557, 556, 455, -1, 557, 455, 456, -1, 456, 558, 557, -1, 559, 558, 456, -1, 559, 456, 457, -1, 560, 559, 457, -1, 561, 560, 457, -1, 457, 458, 561, -1, 562, 561, 458, -1, 458, 563, 562, -1, 563, 458, 459, -1, 564, 563, 459, -1, 459, 565, 564, -1, 459, 460, 565, -1, 460, 566, 565, -1, 567, 566, 460, -1, 460, 107, 567, -1, 107, 568, 567, -1, 107, 569, 568, -1, 107, 570, 569, -1, 570, 107, 106, -1, 571, 570, 106, -1, 106, 572, 571, -1, 106, 110, 572, -1, 110, 573, 572, -1, 110, 574, 573, -1, 110, 113, 574, -1, 113, 575, 574, -1, 576, 575, 113, -1, 576, 113, 117, -1, 117, 577, 576, -1, 578, 577, 117, -1, 578, 117, 116, -1, 579, 578, 116, -1, 116, 580, 579, -1, 116, 120, 580, -1, 581, 580, 120, -1, 120, 582, 581, -1, 583, 582, 120, -1, 583, 120, 454, -1, 454, 584, 583, -1, 585, 584, 454, -1, 585, 454, 452, -1, 586, 585, 452, -1, 587, 586, 452, -1, 452, 450, 587, -1, 450, 537, 587, -1, 475, 473, 588, -1, 473, 589, 588, -1, 477, 475, 590, -1, 588, 590, 475, -1, 591, 479, 477, -1, 590, 591, 477, -1, 481, 479, 592, -1, 591, 592, 479, -1, 593, 483, 481, -1, 592, 593, 481, -1, 594, 485, 483, -1, 483, 593, 594, -1, 487, 485, 595, -1, 485, 594, 595, -1, 596, 490, 487, -1, 487, 595, 596, -1, 597, 492, 490, -1, 490, 596, 597, -1, 598, 494, 492, -1, 597, 598, 492, -1, 495, 494, 599, -1, 494, 598, 599, -1, 498, 495, 600, -1, 599, 600, 495, -1, 500, 498, 601, -1, 600, 601, 498, -1, 501, 500, 602, -1, 500, 601, 602, -1, 603, 504, 501, -1, 602, 603, 501, -1, 505, 504, 604, -1, 504, 603, 604, -1, 605, 507, 505, -1, 505, 604, 605, -1, 510, 507, 606, -1, 507, 605, 606, -1, 512, 510, 607, -1, 510, 606, 607, -1, 608, 511, 512, -1, 512, 607, 608, -1, 509, 511, 609, -1, 608, 609, 511, -1, 508, 509, 610, -1, 609, 610, 509, -1, 506, 508, 611, -1, 610, 611, 508, -1, 611, 612, 506, -1, 612, 503, 506, -1, 612, 613, 503, -1, 613, 502, 503, -1, 613, 614, 502, -1, 614, 499, 502, -1, 614, 615, 499, -1, 615, 497, 499, -1, 615, 616, 497, -1, 496, 497, 616, -1, 616, 617, 496, -1, 617, 493, 496, -1, 617, 618, 493, -1, 491, 493, 618, -1, 618, 619, 491, -1, 619, 489, 491, -1, 620, 488, 489, -1, 489, 619, 620, -1, 486, 488, 621, -1, 488, 620, 621, -1, 622, 484, 486, -1, 486, 621, 622, -1, 623, 482, 484, -1, 484, 622, 623, -1, 624, 480, 482, -1, 623, 624, 482, -1, 478, 480, 625, -1, 480, 624, 625, -1, 476, 478, 626, -1, 625, 626, 478, -1, 627, 474, 476, -1, 626, 627, 476, -1, 471, 474, 628, -1, 474, 627, 628, -1, 629, 469, 471, -1, 628, 629, 471, -1, 467, 469, 630, -1, 469, 629, 630, -1, 631, 465, 467, -1, 467, 630, 631, -1, 464, 465, 632, -1, 465, 631, 632, -1, 461, 464, 633, -1, 464, 632, 633, -1, 634, 462, 461, -1, 461, 633, 634, -1, 463, 462, 635, -1, 634, 635, 462, -1, 466, 463, 636, -1, 635, 636, 463, -1, 636, 637, 466, -1, 468, 466, 637, -1, 637, 638, 468, -1, 638, 470, 468, -1, 638, 639, 470, -1, 639, 472, 470, -1, 472, 639, 589, -1, 589, 473, 472, -1, 71, 75, 167, -1, 167, 75, 91, -1, 73, 91, 75, -1, 514, 72, 71, -1, 640, 514, 438, -1, 514, 640, 641, -1, 642, 640, 438, -1, 643, 640, 642, -1, 644, 640, 643, -1, 641, 640, 644, -1, 70, 517, 102, -1, 70, 645, 517, -1, 70, 646, 645, -1, 68, 646, 70, -1, 102, 69, 70, -1, 517, 645, 647, -1, 647, 645, 42, -1, 648, 26, 647, -1, 648, 42, 26, -1, 648, 647, 42, -1, 26, 649, 647, -1, 647, 649, 516, -1, 650, 517, 647, -1, 650, 516, 517, -1, 650, 647, 516, -1, 651, 515, 516, -1, 651, 516, 649, -1, 652, 651, 649, -1, 653, 651, 652, -1, 515, 651, 440, -1, 440, 651, 653, -1, 654, 440, 653, -1, 655, 97, 98, -1, 655, 98, 20, -1, 656, 97, 655, -1, 656, 95, 97, -1, 7, 95, 656, -1, 98, 94, 93, -1, 98, 99, 94, -1, 73, 74, 92, -1, 657, 93, 92, -1, 92, 658, 657, -1, 657, 659, 93, -1, 657, 660, 659, -1, 661, 660, 657, -1, 658, 661, 657, -1, 531, 518, 662, -1, 67, 518, 65, -1, 662, 518, 67, -1, 662, 67, 64, -1, 66, 65, 663, -1, 663, 65, 530, -1, 63, 61, 87, -1, 87, 61, 86, -1, 86, 61, 85, -1, 85, 61, 82, -1, 82, 61, 83, -1, 83, 61, 84, -1, 84, 61, 522, -1, 61, 523, 522, -1, 61, 524, 523, -1, 61, 525, 524, -1, 61, 526, 525, -1, 61, 527, 526, -1, 61, 528, 527, -1, 61, 520, 528, -1, 61, 62, 520, -1, 62, 519, 520, -1, 62, 521, 519, -1, 62, 529, 521, -1, 62, 530, 529, -1, 664, 59, 88, -1, 664, 665, 59, -1, 88, 666, 664, -1, 667, 666, 668, -1, 654, 666, 667, -1, 664, 666, 654, -1, 88, 77, 666, -1, 668, 666, 77, -1, 668, 77, 76, -1, 669, 668, 76, -1, 76, 670, 669, -1, 670, 76, 79, -1, 532, 670, 79, -1, 79, 80, 532, -1, 642, 438, 531, -1, 531, 662, 642, -1, 642, 671, 643, -1, 643, 671, 644, -1, 644, 671, 641, -1, 641, 671, 662, -1, 662, 671, 642, -1, 672, 667, 668, -1, 672, 668, 669, -1, 669, 673, 672, -1, 673, 669, 670, -1, 532, 534, 673, -1, 673, 670, 532, -1, 667, 674, 654, -1, 674, 667, 672, -1, 674, 672, 673, -1, 673, 675, 674, -1, 534, 535, 675, -1, 675, 673, 534, -1, 535, 440, 676, -1, 674, 676, 654, -1, 675, 676, 674, -1, 675, 535, 676, -1, 654, 676, 440, -1, 95, 677, 69, -1, 69, 677, 678, -1, 69, 678, 31, -1, 678, 677, 679, -1, 679, 677, 95, -1, 679, 95, 9, -1, 680, 537, 536, -1, 680, 536, 538, -1, 680, 538, 539, -1, 680, 539, 540, -1, 680, 540, 541, -1, 680, 541, 542, -1, 680, 542, 543, -1, 680, 543, 544, -1, 680, 544, 545, -1, 680, 545, 546, -1, 680, 546, 547, -1, 680, 547, 548, -1, 680, 548, 549, -1, 680, 549, 550, -1, 680, 550, 551, -1, 680, 551, 552, -1, 680, 552, 553, -1, 680, 553, 554, -1, 680, 554, 555, -1, 556, 680, 555, -1, 557, 680, 556, -1, 558, 680, 557, -1, 559, 680, 558, -1, 560, 680, 559, -1, 561, 680, 560, -1, 562, 680, 561, -1, 563, 680, 562, -1, 564, 680, 563, -1, 565, 680, 564, -1, 566, 680, 565, -1, 567, 680, 566, -1, 568, 680, 567, -1, 568, 569, 680, -1, 569, 570, 680, -1, 570, 571, 680, -1, 571, 572, 680, -1, 572, 573, 680, -1, 573, 574, 680, -1, 574, 575, 680, -1, 575, 576, 680, -1, 576, 577, 680, -1, 577, 578, 680, -1, 578, 579, 680, -1, 579, 580, 680, -1, 580, 581, 680, -1, 581, 582, 680, -1, 680, 582, 583, -1, 680, 583, 584, -1, 680, 584, 585, -1, 680, 585, 586, -1, 680, 586, 587, -1, 680, 587, 537, -1, 589, 681, 588, -1, 588, 681, 590, -1, 590, 681, 591, -1, 591, 681, 592, -1, 592, 681, 593, -1, 593, 681, 594, -1, 594, 681, 595, -1, 681, 596, 595, -1, 681, 597, 596, -1, 681, 598, 597, -1, 681, 599, 598, -1, 681, 600, 599, -1, 681, 601, 600, -1, 681, 602, 601, -1, 681, 603, 602, -1, 681, 604, 603, -1, 681, 605, 604, -1, 681, 606, 605, -1, 681, 607, 606, -1, 681, 608, 607, -1, 681, 609, 608, -1, 681, 610, 609, -1, 681, 611, 610, -1, 681, 612, 611, -1, 681, 613, 612, -1, 681, 614, 613, -1, 681, 615, 614, -1, 681, 616, 615, -1, 681, 617, 616, -1, 681, 618, 617, -1, 681, 619, 618, -1, 681, 620, 619, -1, 621, 620, 681, -1, 622, 621, 681, -1, 623, 622, 681, -1, 624, 623, 681, -1, 625, 624, 681, -1, 626, 625, 681, -1, 627, 626, 681, -1, 628, 627, 681, -1, 629, 628, 681, -1, 630, 629, 681, -1, 631, 630, 681, -1, 632, 631, 681, -1, 633, 632, 681, -1, 633, 681, 634, -1, 634, 681, 635, -1, 635, 681, 636, -1, 636, 681, 637, -1, 637, 681, 638, -1, 638, 681, 639, -1, 639, 681, 589, -1, 72, 92, 74, -1, 72, 514, 92, -1, 58, 658, 92, -1, 55, 658, 58, -1, 641, 662, 58, -1, 682, 56, 683, -1, 58, 682, 683, -1, 56, 58, 683, -1, 58, 662, 682, -1, 514, 641, 58, -1, 58, 92, 514, -1, 42, 645, 646, -1, 68, 52, 646, -1, 28, 52, 68, -1, 52, 42, 646, -1, 31, 29, 69, -1, 29, 28, 69, -1, 56, 682, 33, -1, 33, 684, 56, -1, 42, 33, 685, -1, 42, 685, 26, -1, 685, 682, 64, -1, 685, 64, 26, -1, 33, 682, 685, -1, 64, 66, 686, -1, 687, 688, 64, -1, 64, 686, 687, -1, 688, 689, 64, -1, 66, 690, 686, -1, 665, 664, 691, -1, 689, 692, 64, -1, 691, 664, 693, -1, 664, 654, 693, -1, 694, 654, 21, -1, 695, 654, 694, -1, 696, 654, 695, -1, 693, 654, 696, -1, 26, 652, 649, -1, 697, 690, 66, -1, 22, 653, 652, -1, 22, 654, 653, -1, 26, 22, 652, -1, 21, 654, 22, -1, 698, 26, 64, -1, 692, 698, 64, -1, 66, 663, 697, -1, 698, 699, 26, -1, 699, 25, 26, -1, 20, 98, 700, -1, 700, 701, 20, -1, 20, 701, 702, -1, 20, 702, 19, -1, 701, 703, 702, -1, 12, 655, 20, -1, 12, 656, 655, -1, 12, 7, 656, -1, 11, 7, 12, -1, 704, 705, 706, -1, 706, 705, 707, -1, 705, 7, 707, -1, 708, 7, 709, -1, 709, 7, 11, -1, 710, 7, 708, -1, 711, 7, 710, -1, 707, 7, 711, -1, 712, 703, 701, -1, 701, 713, 712, -1, 9, 95, 6, -1, 7, 6, 95, -1, 700, 98, 93, -1, 700, 93, 714, -1, 93, 659, 714, -1, 55, 714, 659, -1, 659, 660, 715, -1, 660, 661, 715, -1, 661, 658, 715, -1, 658, 55, 715, -1, 55, 659, 715, -1, 64, 682, 662, -1, 663, 530, 716, -1, 717, 663, 716, -1, 717, 716, 718, -1, 718, 719, 717, -1, 718, 720, 719, -1, 721, 719, 720, -1, 721, 720, 722, -1, 722, 723, 721, -1, 723, 722, 724, -1, 725, 723, 724, -1, 725, 724, 726, -1, 726, 727, 725, -1, 727, 726, 728, -1, 728, 729, 727, -1, 728, 730, 729, -1, 728, 731, 730, -1, 62, 716, 530, -1, 62, 718, 716, -1, 60, 720, 718, -1, 60, 718, 62, -1, 60, 722, 720, -1, 724, 722, 60, -1, 726, 724, 60, -1, 728, 726, 60, -1, 731, 728, 60, -1, 732, 731, 60, -1, 733, 732, 60, -1, 734, 733, 60, -1, 735, 60, 59, -1, 735, 734, 60, -1, 730, 731, 732, -1, 736, 730, 732, -1, 736, 732, 733, -1, 737, 736, 733, -1, 733, 734, 737, -1, 734, 738, 737, -1, 738, 734, 735, -1, 739, 738, 735, -1, 665, 739, 59, -1, 739, 735, 59, -1, 740, 28, 27, -1, 741, 740, 27, -1, 27, 742, 741, -1, 742, 27, 30, -1, 678, 742, 30, -1, 30, 31, 678, -1, 743, 740, 741, -1, 743, 744, 740, -1, 741, 745, 743, -1, 745, 741, 742, -1, 678, 679, 745, -1, 745, 742, 678, -1, 744, 5, 7, -1, 5, 744, 743, -1, 745, 8, 5, -1, 5, 743, 745, -1, 679, 9, 8, -1, 8, 745, 679, -1, 57, 714, 55, -1, 714, 57, 700, -1, 684, 33, 37, -1, 684, 37, 701, -1, 57, 684, 701, -1, 700, 57, 701, -1, 56, 684, 57, -1, 701, 37, 713, -1, 713, 37, 43, -1, 4, 2, 50, -1, 50, 2, 49, -1, 49, 2, 48, -1, 48, 2, 46, -1, 46, 2, 47, -1, 47, 2, 53, -1, 53, 2, 54, -1, 2, 45, 54, -1, 2, 44, 45, -1, 2, 41, 44, -1, 2, 39, 41, -1, 2, 36, 39, -1, 2, 34, 36, -1, 2, 32, 34, -1, 2, 3, 32, -1, 3, 35, 32, -1, 3, 38, 35, -1, 3, 40, 38, -1, 3, 43, 40, -1, 705, 704, 0, -1, 705, 0, 51, -1, 744, 746, 740, -1, 7, 746, 744, -1, 705, 746, 7, -1, 51, 746, 705, -1, 51, 28, 746, -1, 740, 746, 28, -1, 747, 665, 691, -1, 748, 665, 747, -1, 748, 747, 749, -1, 747, 691, 693, -1, 747, 693, 696, -1, 747, 696, 695, -1, 747, 695, 694, -1, 747, 694, 21, -1, 747, 21, 23, -1, 747, 23, 24, -1, 747, 24, 25, -1, 747, 25, 699, -1, 747, 699, 698, -1, 747, 698, 692, -1, 747, 692, 689, -1, 747, 689, 688, -1, 687, 747, 688, -1, 749, 747, 687, -1, 749, 687, 686, -1, 690, 749, 686, -1, 697, 749, 690, -1, 663, 749, 697, -1, 750, 704, 706, -1, 751, 704, 750, -1, 751, 750, 752, -1, 750, 706, 707, -1, 750, 707, 711, -1, 750, 711, 710, -1, 750, 710, 708, -1, 750, 708, 709, -1, 750, 709, 11, -1, 750, 11, 10, -1, 750, 10, 13, -1, 750, 13, 14, -1, 750, 14, 15, -1, 750, 15, 16, -1, 750, 16, 17, -1, 750, 17, 18, -1, 19, 750, 18, -1, 752, 750, 19, -1, 752, 19, 702, -1, 703, 752, 702, -1, 712, 752, 703, -1, 713, 752, 712, -1, 717, 749, 663, -1, 719, 749, 717, -1, 721, 748, 719, -1, 719, 748, 749, -1, 723, 748, 721, -1, 723, 725, 748, -1, 725, 727, 748, -1, 727, 729, 748, -1, 729, 730, 748, -1, 748, 730, 736, -1, 748, 736, 737, -1, 748, 737, 738, -1, 748, 739, 665, -1, 748, 738, 739, -1, 713, 43, 753, -1, 754, 713, 753, -1, 754, 753, 755, -1, 755, 756, 754, -1, 755, 757, 756, -1, 758, 756, 757, -1, 758, 757, 759, -1, 759, 760, 758, -1, 760, 759, 761, -1, 762, 760, 761, -1, 762, 761, 763, -1, 763, 764, 762, -1, 764, 763, 765, -1, 765, 766, 764, -1, 765, 767, 766, -1, 765, 768, 767, -1, 3, 753, 43, -1, 3, 755, 753, -1, 1, 757, 755, -1, 1, 755, 3, -1, 1, 759, 757, -1, 761, 759, 1, -1, 763, 761, 1, -1, 765, 763, 1, -1, 768, 765, 1, -1, 769, 768, 1, -1, 770, 769, 1, -1, 771, 770, 1, -1, 772, 1, 0, -1, 772, 771, 1, -1, 767, 768, 769, -1, 773, 767, 769, -1, 773, 769, 770, -1, 774, 773, 770, -1, 770, 771, 774, -1, 771, 775, 774, -1, 775, 771, 772, -1, 776, 775, 772, -1, 704, 776, 0, -1, 776, 772, 0, -1, 754, 752, 713, -1, 756, 752, 754, -1, 758, 751, 756, -1, 756, 751, 752, -1, 760, 751, 758, -1, 760, 762, 751, -1, 762, 764, 751, -1, 764, 766, 751, -1, 766, 767, 751, -1, 751, 767, 773, -1, 751, 773, 774, -1, 751, 774, 775, -1, 751, 776, 704, -1, 751, 775, 776, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 10.500 0.500 64.702 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 10.500 0.500 -123.702 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 10.500 -93.702 -29.500 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 10.500 94.702 -29.500 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -83.702 0.500 -29.500 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 104.702 0.500 -29.500 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_HIP_P.wrl000066400000000000000000001106141207742442300227460ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 45.489 37.750 69.318 center 0.000 0.000 0.000 #translation -13.611 3.975 25.660 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 1.009 -14.100 -60.319, 36.355 10.900 -43.837, 36.355 -14.100 -43.837, 1.009 10.900 -60.319, -9.134 -14.100 -38.567, -9.134 10.900 -38.567, 3.707 -14.100 -49.736, 4.660 -14.100 -50.050, 2.863 -14.100 -49.196, 5.660 -14.100 -50.116, 3.733 -14.100 -42.498, 4.687 -14.100 -42.191, 2.103 -14.100 -36.637, 5.688 -14.100 -42.132, 2.179 -14.100 -48.463, 6.673 -14.100 -42.323, 2.195 -14.100 -43.761, 2.884 -14.100 -43.032, 6.646 -14.100 -49.932, 1.699 -14.100 -47.583, 0.835 -14.100 -33.919, 7.555 -14.100 -49.508, 9.294 -14.100 -47.131, 8.922 -14.100 -48.062, 8.330 -14.100 -48.871, 7.578 -14.100 -42.753, 13.611 -14.100 -41.202, 8.349 -14.100 -43.395, 8.935 -14.100 -44.208, 9.301 -14.100 -45.142, 9.423 -14.100 -46.137, 27.480 -14.100 -24.804, 1.453 -14.100 -46.611, 1.457 -14.100 -45.608, 1.709 -14.100 -44.638, 27.480 10.900 -24.804, 31.284 -1.600 -32.961, 26.212 10.900 -22.085, 26.212 -14.100 -22.085, 10.333 10.900 -52.439, 13.611 10.900 -41.202, 11.740 10.900 -41.214, 10.489 10.900 -39.931, 12.674 10.900 -42.742, 13.244 10.900 -44.440, -0.769 10.900 -41.058, -1.741 10.900 -42.562, 1.863 10.900 -53.287, 3.546 10.900 -53.900, 5.324 10.900 -54.122, 0.358 10.900 -52.315, 7.106 10.900 -53.944, -0.893 10.900 -51.033, 8.804 10.900 -53.374, -1.827 10.900 -49.504, 2.103 10.900 -36.637, 3.740 10.900 -38.302, 2.042 10.900 -38.873, 5.523 10.900 -38.124, 0.514 10.900 -39.807, 0.836 10.900 -33.919, 13.423 10.900 -46.223, 13.200 10.900 -48.000, 12.587 10.900 -49.684, 11.615 10.900 -51.189, -2.398 10.900 -47.806, -2.576 10.900 -46.023, 8.984 10.900 -38.959, 7.301 10.900 -38.346, -2.353 10.900 -44.246, 8.539 -1.600 -30.326, 4.687 -15.100 -42.191, 3.733 -15.100 -42.498, 5.688 -15.100 -42.132, 6.673 -15.100 -42.323, 7.578 -15.100 -42.753, 8.349 -15.100 -43.395, 8.935 -15.100 -44.208, 9.301 -15.100 -45.142, 9.423 -15.100 -46.137, 9.294 -15.100 -47.131, 8.922 -15.100 -48.062, 8.330 -15.100 -48.871, 7.555 -15.100 -49.508, 6.646 -15.100 -49.932, 5.660 -15.100 -50.116, 4.660 -15.100 -50.050, 3.707 -15.100 -49.736, 2.863 -15.100 -49.196, 2.179 -15.100 -48.463, 1.699 -15.100 -47.583, 1.453 -15.100 -46.611, 1.457 -15.100 -45.608, 1.709 -15.100 -44.638, 2.195 -15.100 -43.761, 2.884 -15.100 -43.032, 2.103 -16.100 -36.637, 27.480 -16.100 -24.804, 0.199 -15.100 -32.554, -1.705 -16.100 -28.471, -1.705 -14.100 -28.471, 27.480 12.900 -24.804, 26.212 12.900 -22.085, 23.254 -14.100 -15.741, 25.367 -15.100 -20.273, 23.254 -16.100 -15.741, 2.042 11.900 -38.873, 3.740 11.900 -38.302, 5.523 11.900 -38.124, 7.301 11.900 -38.346, 8.984 11.900 -38.959, 10.489 11.900 -39.931, 11.740 11.900 -41.214, 12.674 11.900 -42.742, 13.244 11.900 -44.440, 13.423 11.900 -46.223, 13.200 11.900 -48.000, 12.587 11.900 -49.684, 11.615 11.900 -51.189, 10.333 11.900 -52.439, 8.804 11.900 -53.374, 7.106 11.900 -53.944, 5.324 11.900 -54.122, 3.546 11.900 -53.900, 1.863 11.900 -53.287, 0.358 11.900 -52.315, -0.893 11.900 -51.033, -1.827 11.900 -49.504, -2.398 11.900 -47.806, -2.576 11.900 -46.023, -2.353 11.900 -44.246, -1.741 11.900 -42.562, -0.769 11.900 -41.058, 0.514 11.900 -39.807, 2.103 12.900 -36.637, 0.836 12.900 -33.919, 7.457 -14.100 4.477, 7.821 -14.100 2.683, 7.250 -14.100 4.381, 6.762 -14.100 -13.501, 6.192 -14.100 -4.065, 7.164 -14.100 -2.561, 4.910 -14.100 -5.316, 3.381 -14.100 -6.250, 1.683 -14.100 -6.821, -0.100 -14.100 -6.999, -1.877 -14.100 -6.777, -3.561 -14.100 -6.164, -5.066 -14.100 -5.192, -6.316 -14.100 -3.910, -7.250 -14.100 -2.381, 7.777 -14.100 -0.877, 7.999 -14.100 0.900, 4.687 -15.100 -42.191, 3.733 -15.100 -42.498, 5.417 -15.100 -46.109, 5.688 -15.100 -42.132, 6.673 -15.100 -42.323, 7.578 -15.100 -42.753, 8.349 -15.100 -43.395, 8.935 -15.100 -44.208, 9.301 -15.100 -45.142, 9.423 -15.100 -46.137, 9.294 -15.100 -47.131, 8.922 -15.100 -48.062, 8.330 -15.100 -48.871, 7.555 -15.100 -49.508, 6.646 -15.100 -49.932, 5.660 -15.100 -50.116, 4.660 -15.100 -50.050, 3.707 -15.100 -49.736, 2.863 -15.100 -49.196, 2.179 -15.100 -48.463, 1.699 -15.100 -47.583, 1.453 -15.100 -46.611, 1.457 -15.100 -45.608, 1.709 -15.100 -44.638, 2.195 -15.100 -43.761, 2.884 -15.100 -43.032, 6.821 -16.100 -4.513, 23.254 -16.100 -15.741, 7.727 -16.100 -1.072, 7.040 -16.100 -2.799, 7.997 -16.100 0.766, 2.103 -16.100 -36.637, 7.000 -16.100 -34.000, -1.705 -16.100 -28.471, 7.835 -16.100 2.617, 7.250 -16.100 4.381, 7.457 -16.100 4.477, 18.148 -16.100 -28.802, 27.480 -16.100 -24.804, -4.326 -16.100 -9.711, -3.379 -16.100 -6.251, -1.615 -16.100 -6.835, -4.961 -16.100 -5.276, 0.236 -16.100 -6.997, -6.275 -16.100 -3.962, 2.074 -16.100 -6.726, -7.250 -16.100 -2.381, 5.974 -16.100 -4.321, 4.586 -16.100 -5.555, -7.250 -16.100 -2.381, -7.250 -14.100 -2.381, -1.705 -16.100 -28.471, -1.705 -14.100 -28.471, 27.480 14.900 -24.804, 23.254 14.900 -15.741, 25.367 13.900 -20.273, 23.254 12.900 -15.741, 23.254 -16.100 -15.741, 23.254 -14.100 -15.741, 7.457 -14.100 4.477, 7.457 -16.100 4.477, 4.994 11.900 -47.026, 3.546 11.900 -53.900, 1.863 11.900 -53.287, 5.232 11.900 -47.105, 5.324 11.900 -54.122, 4.783 11.900 -46.891, 0.358 11.900 -52.315, -0.893 11.900 -51.033, 5.483 11.900 -47.121, 7.106 11.900 -53.944, 4.612 11.900 -46.708, -1.827 11.900 -49.504, 5.729 11.900 -47.075, 8.804 11.900 -53.374, 4.492 11.900 -46.488, -2.398 11.900 -47.806, 5.956 11.900 -46.969, 10.333 11.900 -52.439, 4.431 11.900 -46.245, -2.576 11.900 -46.023, 6.150 11.900 -46.810, 11.615 11.900 -51.189, 4.432 11.900 -45.994, -2.353 11.900 -44.246, 6.298 11.900 -46.608, 12.587 11.900 -49.684, 4.495 11.900 -45.752, 6.391 11.900 -46.375, 13.200 11.900 -48.000, 6.423 11.900 -46.127, -1.741 11.900 -42.562, 4.616 11.900 -45.533, 13.423 11.900 -46.223, -0.769 11.900 -41.058, 4.789 11.900 -45.350, 13.244 11.900 -44.440, 6.393 11.900 -45.878, 0.514 11.900 -39.807, 5.001 11.900 -45.217, 12.674 11.900 -42.742, 6.301 11.900 -45.644, 2.042 11.900 -38.873, 11.740 11.900 -41.214, 6.155 11.900 -45.441, 3.740 11.900 -38.302, 5.239 11.900 -45.140, 10.489 11.900 -39.931, 5.962 11.900 -45.281, 5.523 11.900 -38.124, 5.490 11.900 -45.125, 8.984 11.900 -38.959, 5.736 11.900 -45.173, 7.301 11.900 -38.346, 2.103 14.900 -36.637, 0.199 13.900 -32.554, -1.705 12.900 -28.471, -1.705 14.900 -28.471, 7.250 -14.100 4.381, 7.250 -16.100 4.381, 2.571 -14.100 -2.064, 1.690 -14.100 -2.625, 3.277 -14.100 -1.294, 3.759 -14.100 -0.368, 3.985 -14.100 0.651, 3.939 -14.100 1.695, 3.625 -14.100 2.690, -1.368 -14.100 -2.759, -0.349 -14.100 -2.985, -2.294 -14.100 -2.277, 0.695 -14.100 -2.939, -3.064 -14.100 -1.571, -3.625 -14.100 -0.690, 7.250 12.900 4.381, 7.821 12.900 2.683, 7.457 12.900 4.477, 6.762 12.900 -13.501, 7.164 12.900 -2.561, 6.192 12.900 -4.065, 4.910 12.900 -5.316, 3.381 12.900 -6.250, 1.683 12.900 -6.821, -0.100 12.900 -6.999, -1.877 12.900 -6.777, -3.561 12.900 -6.164, -5.065 12.900 -5.192, -6.316 12.900 -3.910, -7.250 12.900 -2.381, 7.777 12.900 -0.877, 7.999 12.900 0.900, 6.821 -22.850 -4.513, -4.326 -22.850 -9.711, 1.248 -19.475 -7.112, 3.277 -16.100 -1.294, 2.571 -16.100 -2.064, 3.759 -16.100 -0.368, 3.985 -16.100 0.651, 3.939 -16.100 1.695, 3.625 -16.100 2.690, -1.368 -16.100 -2.759, -2.294 -16.100 -2.277, -0.349 -16.100 -2.985, -3.064 -16.100 -1.571, 0.695 -16.100 -2.939, -3.625 -16.100 -0.690, 1.690 -16.100 -2.625, 18.148 -22.850 -28.802, 7.000 -22.850 -34.000, -7.821 -14.100 -0.683, -7.821 -16.100 -0.683, -7.999 -14.100 1.100, -7.999 -16.100 1.100, -7.777 -14.100 2.877, -7.777 -16.100 2.877, -7.164 -14.100 4.561, -7.164 -16.100 4.561, -6.192 -14.100 6.065, -6.192 -16.100 6.065, -4.910 -14.100 7.316, -4.910 -16.100 7.316, -3.381 -14.100 8.250, -3.381 -16.100 8.250, 7.457 14.900 4.477, 7.821 14.900 2.683, 7.250 14.900 4.381, 7.396 14.900 -14.860, 6.192 14.900 -4.065, 7.164 14.900 -2.561, 4.910 14.900 -5.316, 3.381 14.900 -6.250, 1.683 14.900 -6.821, -0.100 14.900 -6.999, -3.561 14.900 -6.164, -1.877 14.900 -6.777, -5.065 14.900 -5.192, -6.316 14.900 -3.910, -7.250 14.900 -2.381, 7.777 14.900 -0.877, 7.999 14.900 0.900, 23.254 12.900 -15.741, 23.254 14.900 -15.741, 7.457 14.900 4.477, 7.457 12.900 4.477, 5.422 11.900 -46.120, -7.250 12.900 -2.381, -7.250 14.900 -2.381, -1.705 12.900 -28.471, -1.705 14.900 -28.471, -1.683 -14.100 8.821, -1.683 -16.100 8.821, 0.100 -14.100 8.999, 0.100 -16.100 8.999, 1.877 -14.100 8.777, 1.877 -16.100 8.777, 3.561 -14.100 8.164, 3.561 -16.100 8.164, 5.065 -14.100 7.192, 5.065 -16.100 7.192, 6.316 -14.100 5.910, 6.316 -16.100 5.910, -3.759 -16.100 2.368, -3.985 -14.100 1.349, -3.759 -14.100 2.368, 3.277 -16.100 -1.294, -3.985 -16.100 1.349, 3.759 -16.100 -0.368, -3.277 -16.100 3.294, -3.277 -14.100 3.294, 2.571 -16.100 -2.064, -0.695 -16.100 4.939, -1.690 -14.100 4.625, -0.695 -14.100 4.939, -1.690 -16.100 4.625, -2.571 -16.100 4.064, -2.571 -14.100 4.064, 0.349 -16.100 4.985, 0.349 -14.100 4.985, 1.690 -16.100 -2.625, 0.695 -16.100 -2.939, 1.368 -16.100 4.759, 1.368 -14.100 4.759, -0.349 -16.100 -2.985, 2.294 -16.100 4.277, 2.294 -14.100 4.277, -1.368 -16.100 -2.759, 3.064 -16.100 3.571, 3.064 -14.100 3.571, -2.294 -16.100 -2.277, 3.625 -16.100 2.690, -3.064 -16.100 -1.571, 3.939 -16.100 1.695, -3.625 -16.100 -0.690, 3.985 -16.100 0.651, -3.939 -16.100 0.305, -3.939 -14.100 0.305, -3.381 -14.100 8.250, -4.910 -14.100 7.316, -1.683 -14.100 8.821, -7.821 -14.100 -0.683, 6.316 -14.100 5.910, -7.999 -14.100 1.100, 0.100 -14.100 8.999, 5.065 -14.100 7.192, -7.777 -14.100 2.877, 3.561 -14.100 8.164, 1.877 -14.100 8.777, -7.164 -14.100 4.561, -6.192 -14.100 6.065, 7.250 14.900 4.381, 7.250 12.900 4.381, 2.571 12.900 -2.064, 1.690 12.900 -2.625, 3.277 12.900 -1.294, 3.759 12.900 -0.368, 3.985 12.900 0.651, 3.939 12.900 1.695, 3.625 12.900 2.690, -1.368 12.900 -2.759, -0.349 12.900 -2.985, -2.294 12.900 -2.277, -3.064 12.900 -1.571, 0.695 12.900 -2.939, -3.625 12.900 -0.690, 6.821 -22.850 -4.513, -4.326 -22.850 -9.711, -1.683 -16.100 8.821, -3.381 -16.100 8.250, -7.821 -16.100 -0.683, -7.250 -16.100 -2.381, 6.316 -16.100 5.910, 7.250 -16.100 4.381, -7.999 -16.100 1.100, 0.100 -16.100 8.999, 5.065 -16.100 7.192, -7.777 -16.100 2.877, 1.877 -16.100 8.777, 3.561 -16.100 8.164, -7.164 -16.100 4.561, -6.192 -16.100 6.065, -4.910 -16.100 7.316, 2.571 14.900 -2.064, 1.690 14.900 -2.625, 3.277 14.900 -1.294, 3.759 14.900 -0.368, 3.985 14.900 0.651, 3.939 14.900 1.695, 3.625 14.900 2.690, -1.368 14.900 -2.759, -0.349 14.900 -2.985, -2.294 14.900 -2.277, 0.695 14.900 -2.939, -3.064 14.900 -1.571, -3.625 14.900 -0.690, -7.821 14.900 -0.683, -7.821 12.900 -0.683, -7.999 14.900 1.100, -7.999 12.900 1.100, -7.777 14.900 2.877, -7.777 12.900 2.877, -7.164 14.900 4.561, -7.164 12.900 4.561, -6.192 14.900 6.065, -6.192 12.900 6.065, -4.910 14.900 7.316, -4.910 12.900 7.316, -3.381 14.900 8.250, -3.381 12.900 8.250, -1.683 14.900 8.821, -1.683 12.900 8.821, 0.100 14.900 8.999, 0.100 12.900 8.999, 1.877 14.900 8.777, 1.877 12.900 8.777, 3.561 14.900 8.164, 3.561 12.900 8.164, 5.066 14.900 7.192, 5.066 12.900 7.192, 6.316 14.900 5.910, 6.316 12.900 5.910, -3.759 12.900 2.368, -3.985 14.900 1.349, -3.759 14.900 2.368, -3.985 12.900 1.349, -3.277 12.900 3.294, -3.277 14.900 3.294, -0.695 12.900 4.939, -1.690 14.900 4.625, -0.695 14.900 4.939, -1.690 12.900 4.625, -2.571 12.900 4.064, -2.571 14.900 4.064, 0.349 12.900 4.985, 0.349 14.900 4.985, 1.368 12.900 4.759, 1.368 14.900 4.759, 2.294 12.900 4.277, 2.294 14.900 4.277, 3.064 12.900 3.571, 3.064 14.900 3.571, -3.939 12.900 0.305, -3.939 14.900 0.305, -1.683 12.900 8.821, -7.821 12.900 -0.683, -3.381 12.900 8.250, 6.316 12.900 5.910, -7.999 12.900 1.100, 0.100 12.900 8.999, 5.066 12.900 7.192, -7.777 12.900 2.877, 1.877 12.900 8.777, 3.561 12.900 8.164, -7.164 12.900 4.561, -6.192 12.900 6.065, -4.910 12.900 7.316, -1.683 14.900 8.821, -3.381 14.900 8.250, -7.821 14.900 -0.683, 6.316 14.900 5.910, -7.999 14.900 1.100, 0.100 14.900 8.999, 5.066 14.900 7.192, -7.777 14.900 2.877, 3.561 14.900 8.164, 1.877 14.900 8.777, -7.164 14.900 4.561, -6.192 14.900 6.065, -4.910 14.900 7.316 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 0, 3, 1, -1, 4, 5, 3, -1, 4, 3, 0, -1, 6, 0, 7, -1, 0, 6, 8, -1, 7, 0, 9, -1, 10, 11, 12, -1, 11, 13, 12, -1, 0, 8, 14, -1, 13, 15, 12, -1, 16, 17, 12, -1, 17, 10, 12, -1, 9, 0, 18, -1, 4, 16, 12, -1, 0, 14, 19, -1, 4, 12, 20, -1, 18, 0, 21, -1, 22, 23, 2, -1, 23, 24, 2, -1, 24, 0, 2, -1, 21, 0, 24, -1, 15, 25, 26, -1, 25, 27, 26, -1, 27, 28, 26, -1, 28, 29, 26, -1, 29, 30, 26, -1, 30, 22, 26, -1, 2, 31, 26, -1, 31, 12, 26, -1, 12, 15, 26, -1, 22, 2, 26, -1, 19, 32, 4, -1, 32, 33, 4, -1, 33, 34, 4, -1, 34, 16, 4, -1, 0, 19, 4, -1, 1, 35, 36, -1, 2, 1, 36, -1, 35, 37, 36, -1, 31, 2, 36, -1, 37, 38, 36, -1, 38, 31, 36, -1, 1, 3, 39, -1, 40, 41, 42, -1, 40, 43, 41, -1, 40, 44, 43, -1, 40, 35, 1, -1, 45, 46, 5, -1, 40, 42, 35, -1, 40, 1, 44, -1, 47, 48, 3, -1, 49, 3, 48, -1, 50, 47, 3, -1, 51, 3, 49, -1, 52, 50, 3, -1, 53, 3, 51, -1, 54, 52, 3, -1, 55, 56, 57, -1, 55, 58, 56, -1, 55, 57, 59, -1, 39, 3, 53, -1, 60, 59, 45, -1, 60, 45, 5, -1, 60, 55, 59, -1, 1, 61, 44, -1, 1, 62, 61, -1, 1, 63, 62, -1, 1, 64, 63, -1, 1, 39, 64, -1, 5, 65, 54, -1, 5, 66, 65, -1, 35, 67, 68, -1, 5, 69, 66, -1, 35, 42, 67, -1, 5, 46, 69, -1, 5, 54, 3, -1, 55, 35, 58, -1, 58, 35, 68, -1, 60, 5, 4, -1, 20, 60, 4, -1, 20, 38, 70, -1, 37, 60, 70, -1, 38, 37, 70, -1, 60, 20, 70, -1, 11, 10, 71, -1, 10, 72, 71, -1, 13, 11, 73, -1, 11, 71, 73, -1, 15, 13, 74, -1, 13, 73, 74, -1, 25, 15, 75, -1, 15, 74, 75, -1, 27, 25, 76, -1, 25, 75, 76, -1, 28, 27, 77, -1, 27, 76, 77, -1, 29, 28, 78, -1, 28, 77, 78, -1, 30, 29, 79, -1, 29, 78, 79, -1, 22, 30, 80, -1, 30, 79, 80, -1, 23, 22, 81, -1, 22, 80, 81, -1, 24, 23, 82, -1, 23, 81, 82, -1, 24, 82, 83, -1, 21, 24, 83, -1, 21, 83, 84, -1, 18, 21, 84, -1, 18, 84, 85, -1, 9, 18, 85, -1, 9, 85, 86, -1, 7, 9, 86, -1, 6, 7, 87, -1, 7, 86, 87, -1, 8, 6, 88, -1, 6, 87, 88, -1, 14, 8, 89, -1, 8, 88, 89, -1, 19, 14, 90, -1, 14, 89, 90, -1, 32, 19, 91, -1, 19, 90, 91, -1, 32, 91, 33, -1, 33, 91, 92, -1, 34, 33, 93, -1, 33, 92, 93, -1, 34, 93, 16, -1, 16, 93, 94, -1, 17, 16, 95, -1, 16, 94, 95, -1, 10, 17, 72, -1, 17, 95, 72, -1, 96, 12, 31, -1, 96, 31, 97, -1, 20, 12, 96, -1, 98, 96, 99, -1, 98, 99, 100, -1, 98, 100, 20, -1, 98, 20, 96, -1, 101, 102, 37, -1, 35, 101, 37, -1, 97, 31, 38, -1, 38, 103, 104, -1, 105, 97, 104, -1, 103, 105, 104, -1, 97, 38, 104, -1, 106, 57, 107, -1, 107, 57, 56, -1, 108, 107, 58, -1, 107, 56, 58, -1, 109, 108, 68, -1, 108, 58, 68, -1, 110, 109, 67, -1, 109, 68, 67, -1, 110, 67, 111, -1, 111, 67, 42, -1, 112, 111, 41, -1, 111, 42, 41, -1, 113, 112, 43, -1, 112, 41, 43, -1, 114, 113, 44, -1, 113, 43, 44, -1, 115, 114, 61, -1, 114, 44, 61, -1, 115, 61, 116, -1, 116, 61, 62, -1, 116, 62, 63, -1, 117, 116, 63, -1, 117, 63, 64, -1, 118, 117, 64, -1, 118, 64, 119, -1, 119, 64, 39, -1, 120, 119, 53, -1, 119, 39, 53, -1, 121, 120, 51, -1, 120, 53, 51, -1, 122, 121, 49, -1, 121, 51, 49, -1, 123, 122, 48, -1, 122, 49, 48, -1, 124, 123, 47, -1, 123, 48, 47, -1, 125, 124, 50, -1, 124, 47, 50, -1, 125, 50, 126, -1, 126, 50, 52, -1, 127, 126, 54, -1, 126, 52, 54, -1, 128, 127, 65, -1, 127, 54, 65, -1, 129, 128, 66, -1, 128, 65, 66, -1, 130, 129, 69, -1, 129, 66, 69, -1, 131, 130, 46, -1, 130, 69, 46, -1, 132, 131, 45, -1, 131, 46, 45, -1, 133, 132, 59, -1, 132, 45, 59, -1, 133, 59, 106, -1, 106, 59, 57, -1, 55, 134, 101, -1, 55, 101, 35, -1, 60, 135, 134, -1, 60, 134, 55, -1, 136, 137, 138, -1, 136, 103, 137, -1, 139, 103, 38, -1, 139, 140, 141, -1, 139, 142, 140, -1, 139, 143, 142, -1, 139, 144, 143, -1, 139, 145, 144, -1, 139, 146, 145, -1, 139, 20, 100, -1, 139, 38, 20, -1, 139, 100, 146, -1, 139, 141, 103, -1, 147, 146, 100, -1, 148, 147, 100, -1, 149, 148, 100, -1, 150, 149, 100, -1, 151, 103, 141, -1, 152, 103, 151, -1, 137, 103, 152, -1, 102, 135, 60, -1, 37, 102, 60, -1, 153, 154, 155, -1, 156, 153, 155, -1, 157, 156, 155, -1, 158, 157, 155, -1, 159, 158, 155, -1, 160, 159, 155, -1, 161, 160, 155, -1, 162, 161, 155, -1, 163, 162, 155, -1, 164, 163, 155, -1, 165, 164, 155, -1, 166, 165, 155, -1, 167, 166, 155, -1, 168, 167, 155, -1, 169, 168, 155, -1, 170, 169, 155, -1, 171, 170, 155, -1, 172, 171, 155, -1, 173, 172, 155, -1, 174, 173, 155, -1, 175, 174, 155, -1, 176, 175, 155, -1, 177, 176, 155, -1, 178, 177, 155, -1, 154, 178, 155, -1, 179, 180, 181, -1, 182, 179, 181, -1, 181, 180, 183, -1, 184, 185, 186, -1, 183, 180, 187, -1, 188, 187, 189, -1, 190, 184, 191, -1, 185, 184, 190, -1, 187, 180, 189, -1, 180, 179, 190, -1, 186, 185, 192, -1, 193, 192, 194, -1, 190, 191, 180, -1, 192, 193, 195, -1, 194, 192, 196, -1, 192, 195, 197, -1, 196, 192, 198, -1, 186, 192, 199, -1, 192, 197, 199, -1, 200, 201, 179, -1, 200, 179, 182, -1, 202, 203, 204, -1, 203, 205, 204, -1, 101, 206, 102, -1, 206, 207, 208, -1, 207, 209, 208, -1, 209, 102, 208, -1, 102, 206, 208, -1, 210, 211, 212, -1, 210, 212, 213, -1, 214, 215, 216, -1, 217, 218, 215, -1, 217, 215, 214, -1, 219, 216, 220, -1, 219, 220, 221, -1, 219, 214, 216, -1, 222, 223, 218, -1, 222, 218, 217, -1, 224, 221, 225, -1, 224, 219, 221, -1, 226, 227, 223, -1, 226, 223, 222, -1, 228, 225, 229, -1, 228, 224, 225, -1, 230, 231, 227, -1, 230, 227, 226, -1, 232, 229, 233, -1, 232, 228, 229, -1, 234, 235, 231, -1, 234, 231, 230, -1, 236, 232, 233, -1, 236, 233, 237, -1, 238, 239, 235, -1, 238, 235, 234, -1, 240, 236, 237, -1, 241, 239, 238, -1, 242, 241, 243, -1, 242, 239, 241, -1, 244, 240, 237, -1, 244, 245, 240, -1, 246, 242, 243, -1, 247, 245, 244, -1, 247, 248, 245, -1, 249, 243, 250, -1, 249, 246, 243, -1, 251, 248, 247, -1, 251, 252, 248, -1, 253, 250, 254, -1, 253, 249, 250, -1, 255, 252, 251, -1, 256, 254, 257, -1, 256, 253, 254, -1, 258, 259, 252, -1, 258, 252, 255, -1, 260, 257, 261, -1, 260, 256, 257, -1, 262, 263, 259, -1, 262, 259, 258, -1, 264, 261, 265, -1, 264, 260, 261, -1, 266, 265, 263, -1, 266, 264, 265, -1, 266, 263, 262, -1, 134, 267, 206, -1, 134, 206, 101, -1, 135, 267, 134, -1, 268, 135, 269, -1, 268, 270, 267, -1, 268, 269, 270, -1, 268, 267, 135, -1, 212, 271, 213, -1, 213, 271, 272, -1, 273, 140, 142, -1, 273, 142, 274, -1, 275, 140, 273, -1, 141, 140, 275, -1, 276, 141, 275, -1, 151, 141, 276, -1, 277, 151, 276, -1, 152, 277, 278, -1, 152, 151, 277, -1, 137, 278, 279, -1, 280, 146, 147, -1, 137, 152, 278, -1, 138, 137, 279, -1, 281, 145, 146, -1, 281, 146, 280, -1, 282, 147, 148, -1, 282, 280, 147, -1, 283, 144, 145, -1, 283, 145, 281, -1, 284, 148, 149, -1, 284, 282, 148, -1, 274, 142, 143, -1, 274, 143, 144, -1, 274, 144, 283, -1, 285, 149, 150, -1, 285, 284, 149, -1, 286, 287, 288, -1, 287, 209, 288, -1, 135, 102, 289, -1, 102, 209, 289, -1, 269, 135, 289, -1, 290, 291, 289, -1, 291, 292, 289, -1, 292, 293, 289, -1, 293, 294, 289, -1, 294, 295, 289, -1, 295, 296, 289, -1, 296, 269, 289, -1, 209, 290, 289, -1, 269, 296, 297, -1, 269, 297, 298, -1, 269, 298, 299, -1, 269, 299, 300, -1, 290, 209, 301, -1, 301, 209, 302, -1, 302, 209, 287, -1, 179, 201, 303, -1, 192, 304, 305, -1, 304, 303, 305, -1, 201, 198, 305, -1, 198, 192, 305, -1, 303, 201, 305, -1, 200, 182, 306, -1, 307, 200, 306, -1, 306, 182, 308, -1, 308, 182, 181, -1, 308, 181, 309, -1, 310, 309, 183, -1, 309, 181, 183, -1, 311, 310, 187, -1, 193, 194, 312, -1, 310, 183, 187, -1, 311, 187, 188, -1, 195, 193, 313, -1, 193, 312, 313, -1, 194, 196, 314, -1, 312, 194, 314, -1, 197, 195, 315, -1, 195, 313, 315, -1, 196, 198, 316, -1, 314, 196, 316, -1, 199, 197, 317, -1, 197, 315, 317, -1, 198, 201, 318, -1, 316, 198, 318, -1, 201, 200, 307, -1, 318, 201, 307, -1, 319, 190, 303, -1, 303, 190, 179, -1, 190, 320, 185, -1, 319, 320, 190, -1, 304, 185, 320, -1, 192, 185, 304, -1, 321, 203, 322, -1, 203, 202, 322, -1, 323, 321, 324, -1, 321, 322, 324, -1, 323, 324, 325, -1, 325, 324, 326, -1, 327, 325, 328, -1, 325, 326, 328, -1, 329, 327, 330, -1, 327, 328, 330, -1, 331, 329, 332, -1, 329, 330, 332, -1, 333, 331, 334, -1, 331, 332, 334, -1, 335, 336, 337, -1, 335, 207, 336, -1, 338, 206, 267, -1, 338, 207, 206, -1, 338, 267, 270, -1, 338, 339, 340, -1, 338, 341, 339, -1, 338, 342, 341, -1, 338, 343, 342, -1, 338, 344, 343, -1, 345, 346, 270, -1, 338, 346, 344, -1, 338, 270, 346, -1, 338, 340, 207, -1, 347, 345, 270, -1, 348, 347, 270, -1, 349, 348, 270, -1, 350, 207, 340, -1, 351, 207, 350, -1, 336, 207, 351, -1, 352, 353, 354, -1, 352, 354, 355, -1, 356, 252, 259, -1, 356, 259, 263, -1, 356, 263, 265, -1, 356, 265, 261, -1, 356, 261, 257, -1, 356, 257, 254, -1, 356, 254, 250, -1, 356, 250, 243, -1, 356, 243, 241, -1, 356, 241, 238, -1, 356, 238, 234, -1, 356, 234, 230, -1, 356, 230, 226, -1, 356, 226, 222, -1, 356, 222, 217, -1, 356, 217, 214, -1, 356, 214, 219, -1, 356, 219, 224, -1, 356, 224, 228, -1, 356, 228, 232, -1, 356, 232, 236, -1, 356, 236, 240, -1, 356, 240, 245, -1, 356, 245, 248, -1, 356, 248, 252, -1, 357, 358, 359, -1, 358, 360, 359, -1, 361, 333, 362, -1, 333, 334, 362, -1, 361, 362, 363, -1, 363, 362, 364, -1, 363, 364, 365, -1, 365, 364, 366, -1, 365, 366, 367, -1, 367, 366, 368, -1, 367, 368, 369, -1, 369, 368, 370, -1, 369, 370, 371, -1, 371, 370, 372, -1, 371, 372, 271, -1, 271, 372, 272, -1, 373, 374, 375, -1, 376, 276, 275, -1, 373, 377, 374, -1, 376, 378, 276, -1, 379, 375, 380, -1, 379, 373, 375, -1, 381, 275, 273, -1, 382, 383, 384, -1, 382, 385, 383, -1, 381, 376, 275, -1, 386, 380, 387, -1, 386, 379, 380, -1, 388, 384, 389, -1, 390, 273, 274, -1, 388, 382, 384, -1, 390, 381, 273, -1, 385, 387, 383, -1, 385, 386, 387, -1, 391, 274, 283, -1, 392, 389, 393, -1, 391, 390, 274, -1, 392, 388, 389, -1, 394, 391, 283, -1, 394, 283, 281, -1, 395, 393, 396, -1, 395, 392, 393, -1, 397, 394, 281, -1, 397, 281, 280, -1, 398, 396, 399, -1, 398, 395, 396, -1, 400, 397, 280, -1, 400, 280, 282, -1, 401, 399, 279, -1, 401, 398, 399, -1, 402, 400, 282, -1, 402, 282, 284, -1, 403, 279, 278, -1, 403, 401, 279, -1, 404, 402, 284, -1, 404, 284, 285, -1, 405, 278, 277, -1, 406, 285, 407, -1, 405, 403, 278, -1, 406, 404, 285, -1, 378, 277, 276, -1, 377, 407, 374, -1, 377, 406, 407, -1, 378, 405, 277, -1, 408, 383, 409, -1, 410, 384, 383, -1, 285, 150, 411, -1, 410, 383, 408, -1, 412, 279, 399, -1, 412, 138, 279, -1, 407, 411, 413, -1, 407, 285, 411, -1, 414, 389, 384, -1, 414, 384, 410, -1, 374, 407, 413, -1, 415, 399, 396, -1, 415, 412, 399, -1, 416, 374, 413, -1, 417, 396, 393, -1, 375, 374, 416, -1, 417, 415, 396, -1, 418, 393, 389, -1, 419, 375, 416, -1, 418, 417, 393, -1, 418, 389, 414, -1, 380, 375, 419, -1, 420, 387, 380, -1, 420, 380, 419, -1, 409, 383, 387, -1, 409, 387, 420, -1, 354, 421, 355, -1, 355, 421, 422, -1, 292, 291, 423, -1, 424, 292, 423, -1, 423, 291, 425, -1, 425, 291, 290, -1, 425, 290, 426, -1, 426, 290, 301, -1, 426, 301, 427, -1, 428, 427, 302, -1, 427, 301, 302, -1, 429, 428, 287, -1, 297, 296, 430, -1, 428, 302, 287, -1, 429, 287, 286, -1, 296, 295, 431, -1, 430, 296, 431, -1, 298, 297, 432, -1, 297, 430, 432, -1, 299, 298, 433, -1, 298, 432, 433, -1, 295, 294, 434, -1, 431, 295, 434, -1, 293, 292, 424, -1, 294, 293, 424, -1, 434, 294, 424, -1, 300, 299, 435, -1, 299, 433, 435, -1, 320, 319, 436, -1, 437, 320, 436, -1, 385, 382, 438, -1, 439, 385, 438, -1, 440, 441, 404, -1, 398, 401, 442, -1, 401, 443, 442, -1, 444, 440, 406, -1, 440, 404, 406, -1, 382, 388, 445, -1, 438, 382, 445, -1, 444, 406, 377, -1, 395, 398, 446, -1, 398, 442, 446, -1, 444, 377, 447, -1, 388, 392, 448, -1, 445, 388, 448, -1, 447, 377, 373, -1, 392, 395, 449, -1, 395, 446, 449, -1, 447, 373, 450, -1, 448, 392, 449, -1, 450, 373, 379, -1, 379, 386, 451, -1, 450, 379, 451, -1, 386, 385, 452, -1, 451, 386, 452, -1, 452, 385, 439, -1, 453, 339, 341, -1, 453, 341, 454, -1, 455, 339, 453, -1, 340, 339, 455, -1, 456, 340, 455, -1, 350, 340, 456, -1, 457, 350, 456, -1, 351, 457, 458, -1, 351, 350, 457, -1, 336, 458, 459, -1, 460, 346, 345, -1, 336, 351, 458, -1, 337, 336, 459, -1, 461, 344, 346, -1, 461, 346, 460, -1, 462, 345, 347, -1, 462, 460, 345, -1, 463, 343, 344, -1, 463, 344, 461, -1, 464, 347, 348, -1, 464, 462, 347, -1, 454, 341, 342, -1, 454, 342, 343, -1, 454, 343, 463, -1, 465, 348, 349, -1, 465, 464, 348, -1, 466, 358, 467, -1, 358, 357, 467, -1, 468, 466, 469, -1, 466, 467, 469, -1, 468, 469, 470, -1, 470, 469, 471, -1, 472, 470, 473, -1, 470, 471, 473, -1, 474, 472, 475, -1, 472, 473, 475, -1, 476, 474, 477, -1, 474, 475, 477, -1, 478, 476, 479, -1, 476, 477, 479, -1, 480, 478, 481, -1, 478, 479, 481, -1, 480, 481, 482, -1, 482, 481, 483, -1, 482, 483, 484, -1, 484, 483, 485, -1, 484, 485, 486, -1, 486, 485, 487, -1, 486, 487, 488, -1, 488, 487, 489, -1, 488, 489, 490, -1, 490, 489, 491, -1, 490, 491, 421, -1, 421, 491, 422, -1, 492, 493, 494, -1, 425, 456, 455, -1, 492, 495, 493, -1, 425, 426, 456, -1, 496, 494, 497, -1, 496, 492, 494, -1, 423, 455, 453, -1, 498, 499, 500, -1, 498, 501, 499, -1, 423, 425, 455, -1, 502, 497, 503, -1, 502, 496, 497, -1, 504, 500, 505, -1, 424, 453, 454, -1, 499, 502, 503, -1, 504, 498, 500, -1, 424, 423, 453, -1, 501, 502, 499, -1, 434, 454, 463, -1, 506, 505, 507, -1, 434, 424, 454, -1, 506, 504, 505, -1, 431, 434, 463, -1, 431, 463, 461, -1, 508, 507, 509, -1, 508, 506, 507, -1, 430, 431, 461, -1, 430, 461, 460, -1, 510, 509, 511, -1, 510, 508, 509, -1, 432, 430, 460, -1, 432, 460, 462, -1, 429, 511, 459, -1, 429, 510, 511, -1, 433, 462, 464, -1, 433, 432, 462, -1, 428, 459, 458, -1, 435, 464, 465, -1, 428, 429, 459, -1, 435, 433, 464, -1, 427, 458, 457, -1, 512, 465, 513, -1, 512, 435, 465, -1, 427, 428, 458, -1, 426, 457, 456, -1, 495, 513, 493, -1, 495, 512, 513, -1, 426, 427, 457, -1, 501, 498, 514, -1, 515, 300, 435, -1, 516, 501, 514, -1, 510, 429, 517, -1, 429, 286, 517, -1, 518, 515, 512, -1, 515, 435, 512, -1, 498, 504, 519, -1, 514, 498, 519, -1, 518, 512, 495, -1, 508, 510, 520, -1, 510, 517, 520, -1, 518, 495, 521, -1, 504, 506, 522, -1, 521, 495, 492, -1, 519, 504, 522, -1, 506, 508, 523, -1, 521, 492, 524, -1, 508, 520, 523, -1, 522, 506, 523, -1, 524, 492, 496, -1, 496, 502, 525, -1, 524, 496, 525, -1, 502, 501, 526, -1, 525, 502, 526, -1, 526, 501, 516, -1, 527, 500, 499, -1, 527, 499, 528, -1, 465, 349, 529, -1, 530, 459, 511, -1, 530, 337, 459, -1, 513, 529, 531, -1, 513, 465, 529, -1, 532, 505, 500, -1, 532, 500, 527, -1, 493, 513, 531, -1, 533, 511, 509, -1, 533, 530, 511, -1, 534, 493, 531, -1, 535, 509, 507, -1, 494, 493, 534, -1, 535, 533, 509, -1, 536, 507, 505, -1, 536, 535, 507, -1, 537, 494, 534, -1, 536, 505, 532, -1, 497, 494, 537, -1, 538, 503, 497, -1, 538, 497, 537, -1, 539, 499, 503, -1, 539, 503, 538, -1, 528, 499, 539, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 13.611 -3.975 65.441 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 13.611 -3.975 -116.760 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 13.611 -95.075 -25.660 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 13.611 87.125 -25.660 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -77.490 -3.975 -25.660 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 104.711 -3.975 -25.660 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_HIP_R.wrl000066400000000000000000000702011207742442300227450ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 46.500 27.000 43.000 center 0.000 0.000 0.000 #translation -12.760 0.500 9.520 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.60 0.30 0.30 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 14.010 -11.000 11.980, 14.010 12.000 11.980, -9.490 -11.000 11.980, -9.490 12.000 11.980, 14.010 12.000 6.980, 14.010 12.000 11.980, 14.010 -11.000 11.980, 14.010 -11.000 6.980, 2.260 12.000 -7.020, -9.490 12.000 -26.020, 14.010 12.000 -26.020, -9.490 -11.000 -26.020, -9.490 -5.755 -3.008, -9.490 -4.488 -4.275, -9.490 -6.708 -1.491, -9.490 -7.299 0.200, -9.490 -7.500 1.980, -9.490 -11.000 11.980, -9.490 0.500 -7.020, -9.490 2.280 -5.819, -9.490 3.971 -5.228, -9.490 -7.299 3.760, -9.490 -6.708 5.451, -9.490 0.500 -6.020, -9.490 -1.280 -5.819, -9.490 6.755 6.968, -9.490 5.488 8.235, -9.490 -2.971 -5.228, -9.490 3.971 9.188, -9.490 7.708 5.451, -9.490 2.280 9.779, -9.490 8.299 3.760, -9.490 0.500 9.980, -9.490 8.500 1.980, -9.490 8.299 0.200, -9.490 7.708 -1.491, -9.490 6.755 -3.008, -9.490 5.488 -4.275, -9.490 -4.488 8.235, -9.490 -5.755 6.968, -9.490 -2.971 9.188, -9.490 -1.280 9.779, 2.260 -11.000 -7.020, 14.010 -11.000 -26.020, 14.010 -13.000 6.980, 18.000 -13.000 6.980, 14.010 -11.000 6.980, 35.010 -13.000 6.980, 35.010 12.000 6.980, 24.510 -0.500 6.980, 14.010 12.000 6.980, 29.000 -13.000 6.980, 20.539 12.000 -10.228, 22.230 12.000 -10.819, 17.755 12.000 1.968, 19.022 12.000 3.235, 19.022 12.000 -9.275, 17.755 12.000 -8.008, 16.802 12.000 -6.491, 28.998 12.000 3.235, 35.010 12.000 6.980, 30.265 12.000 1.968, 14.010 12.000 -31.020, 35.010 12.000 -31.020, 16.211 12.000 -4.800, 16.010 12.000 -3.020, 31.218 12.000 0.451, 24.510 12.000 -12.020, 25.790 12.000 -10.819, 27.481 12.000 -10.228, 24.010 12.000 -11.020, 27.481 12.000 4.188, 16.802 12.000 0.451, 31.809 12.000 -1.240, 16.211 12.000 -1.240, 25.790 12.000 4.779, 24.010 12.000 4.980, 32.010 12.000 -3.020, 31.809 12.000 -4.800, 31.218 12.000 -6.491, 30.265 12.000 -8.008, 28.998 12.000 -9.275, 20.539 12.000 4.188, 22.230 12.000 4.779, -10.490 0.500 9.980, -10.490 2.280 9.779, -10.490 3.971 9.188, -10.490 5.488 8.235, -10.490 6.755 6.968, -10.490 7.708 5.451, -10.490 8.299 3.760, -10.490 8.500 1.980, -10.490 8.299 0.200, -10.490 7.708 -1.491, -10.490 6.755 -3.008, -10.490 5.488 -4.275, -10.490 3.971 -5.228, -10.490 2.280 -5.819, -10.490 0.500 -6.020, -10.490 -1.280 -5.819, -10.490 -2.971 -5.228, -10.490 -4.488 -4.275, -10.490 -5.755 -3.008, -10.490 -6.708 -1.491, -10.490 -7.299 0.200, -10.490 -7.500 1.980, -10.490 -7.299 3.760, -10.490 -6.708 5.451, -10.490 -5.755 6.968, -10.490 -4.488 8.235, -10.490 -2.971 9.188, -10.490 -1.280 9.779, 14.010 -13.000 6.980, 14.010 -13.000 -31.020, 35.010 3.238 4.896, 35.010 3.877 4.123, 35.010 2.427 5.485, 35.010 4.304 3.216, 35.010 1.495 5.854, 35.010 4.492 2.231, 35.010 -2.582 -0.570, 35.010 -3.119 0.277, 35.010 -13.000 6.980, 35.010 -3.429 1.230, 35.010 -3.492 2.231, 35.010 -3.304 3.216, 35.010 -2.877 4.123, 35.010 -2.238 4.896, 35.010 0.500 5.980, 35.010 -1.427 5.485, 35.010 -0.495 5.854, 35.010 4.429 1.230, 35.010 3.582 -0.570, 35.010 4.119 0.277, 35.010 2.851 -1.256, 35.010 -0.500 -12.020, 35.010 1.972 -1.739, 35.010 1.001 -1.988, 35.010 -0.001 -1.988, 35.010 -0.972 -1.739, 35.010 -1.851 -1.256, 35.010 -13.000 -31.020, 29.000 -13.000 -3.000, 35.010 -13.000 -31.020, 27.464 -13.000 -5.000, 21.172 -13.000 -5.828, 18.000 -13.000 -3.000, 14.010 -13.000 -31.020, 22.000 -13.000 -6.464, 24.510 -13.000 -12.020, 22.965 -13.000 -6.864, 24.000 -13.000 -7.000, 28.000 -13.000 -3.000, 27.864 -13.000 -4.035, 25.035 -13.000 -6.864, 26.000 -13.000 -6.464, 26.828 -13.000 -5.828, 20.136 -13.000 -4.035, 20.000 -13.000 -3.000, 20.536 -13.000 -5.000, 23.832 -13.000 5.976, 21.119 -13.000 4.745, 20.517 -13.000 3.928, 20.139 -13.000 2.987, 26.643 -13.000 4.991, 27.316 -13.000 4.232, 25.801 -13.000 5.557, 20.010 -13.000 1.980, 27.776 -13.000 3.327, 24.843 -13.000 5.892, 27.994 -13.000 2.336, 20.875 -13.000 -0.504, 20.402 -13.000 0.252, 20.397 -13.000 -1.263, 27.663 -13.000 0.351, 27.135 -13.000 -0.516, 27.608 -13.000 -1.272, 27.956 -13.000 1.322, 20.101 -13.000 -2.109, 20.109 -13.000 1.094, 27.901 -13.000 -2.114, 22.832 -13.000 5.803, 21.908 -13.000 5.383, 24.010 13.000 4.980, 25.790 13.000 4.779, 27.481 13.000 4.188, 28.998 13.000 3.235, 30.265 13.000 1.968, 31.218 13.000 0.451, 31.809 13.000 -1.240, 32.010 13.000 -3.020, 31.809 13.000 -4.800, 31.218 13.000 -6.491, 30.265 13.000 -8.008, 28.998 13.000 -9.275, 27.481 13.000 -10.228, 25.790 13.000 -10.819, 24.010 13.000 -11.020, 22.230 13.000 -10.819, 20.539 13.000 -10.228, 19.022 13.000 -9.275, 17.755 13.000 -8.008, 16.802 13.000 -6.491, 16.211 13.000 -4.800, 16.010 13.000 -3.020, 16.211 13.000 -1.240, 16.802 13.000 0.451, 17.755 13.000 1.968, 19.022 13.000 3.235, 20.539 13.000 4.188, 22.230 13.000 4.779, 35.010 12.000 -31.020, 14.010 -13.000 -31.020, -10.490 -0.271 1.343, -10.490 -4.488 -4.275, -10.490 -5.755 -3.008, -10.490 -0.088 1.171, -10.490 -0.405 1.554, -10.490 -6.708 -1.491, -10.490 -7.299 0.200, -10.490 0.132 1.050, -10.490 -0.482 1.793, -10.490 -7.500 1.980, -10.490 0.375 0.988, -10.490 -0.498 2.043, -10.490 -7.299 3.760, -10.490 0.625 0.988, -10.490 2.280 -5.819, -10.490 -0.451 2.289, -10.490 -6.708 5.451, -10.490 0.868 1.050, -10.490 3.971 -5.228, -10.490 -0.344 2.516, -10.490 -5.755 6.968, -10.490 1.088 1.171, -10.490 5.488 -4.275, -10.490 -0.185 2.709, -10.490 1.271 1.343, -10.490 6.755 -3.008, -10.490 1.405 1.554, -10.490 -4.488 8.235, -10.490 0.018 2.856, -10.490 7.708 -1.491, -10.490 0.251 2.949, -10.490 8.299 0.200, -10.490 1.482 1.793, -10.490 0.500 2.980, -10.490 8.500 1.980, -10.490 1.498 2.043, -10.490 8.299 3.760, -10.490 1.451 2.289, -10.490 2.280 9.779, -10.490 0.749 2.949, -10.490 7.708 5.451, -10.490 1.344 2.516, -10.490 3.971 9.188, -10.490 0.982 2.856, -10.490 6.755 6.968, -10.490 1.185 2.709, -10.490 5.488 8.235, 36.010 0.500 5.980, 36.010 1.495 5.854, 36.010 2.427 5.485, 36.010 3.238 4.896, 36.010 3.877 4.123, 36.010 4.304 3.216, 36.010 4.492 2.231, 36.010 4.429 1.230, 36.010 4.119 0.277, 36.010 3.582 -0.570, 36.010 2.851 -1.256, 36.010 1.972 -1.739, 36.010 1.001 -1.988, 36.010 -0.001 -1.988, 36.010 -0.972 -1.739, 36.010 -1.851 -1.256, 36.010 -2.582 -0.570, 36.010 -3.119 0.277, 36.010 -3.429 1.230, 36.010 -3.492 2.231, 36.010 -3.304 3.216, 36.010 -2.877 4.123, 36.010 -2.238 4.896, 36.010 -1.427 5.485, 36.010 -0.495 5.854, 28.000 -14.000 -3.000, 27.874 -14.000 -2.005, 27.505 -14.000 -1.073, 26.916 -14.000 -0.262, 26.402 -13.000 0.199, 26.143 -14.000 0.377, 25.511 -13.000 0.704, 25.236 -14.000 0.804, 24.520 -13.000 0.966, 24.251 -14.000 0.992, 23.496 -13.000 0.968, 23.250 -14.000 0.929, 22.504 -13.000 0.710, 22.297 -14.000 0.619, 21.611 -13.000 0.208, 21.450 -14.000 0.082, 20.764 -14.000 -0.649, 20.281 -14.000 -1.528, 20.032 -14.000 -2.499, 20.032 -14.000 -3.501, 20.281 -14.000 -4.472, 20.764 -14.000 -5.351, 21.450 -14.000 -6.082, 22.297 -14.000 -6.619, 23.250 -14.000 -6.929, 24.251 -14.000 -6.992, 25.236 -14.000 -6.804, 26.143 -14.000 -6.377, 26.916 -14.000 -5.738, 27.505 -14.000 -4.927, 27.874 -14.000 -3.995, 24.002 -13.000 2.730, 23.239 13.000 -3.657, 19.022 13.000 -9.275, 17.755 13.000 -8.008, 23.422 13.000 -3.829, 20.539 13.000 -10.228, 23.105 13.000 -3.446, 16.802 13.000 -6.491, 16.211 13.000 -4.800, 23.642 13.000 -3.950, 22.230 13.000 -10.819, 23.028 13.000 -3.207, 16.010 13.000 -3.020, 23.885 13.000 -4.012, 24.010 13.000 -11.020, 23.012 13.000 -2.957, 16.211 13.000 -1.240, 24.135 13.000 -4.012, 25.790 13.000 -10.819, 23.059 13.000 -2.711, 16.802 13.000 0.451, 24.378 13.000 -3.950, 27.481 13.000 -10.228, 23.166 13.000 -2.484, 17.755 13.000 1.968, 24.598 13.000 -3.829, 23.325 13.000 -2.291, 24.781 13.000 -3.657, 30.265 13.000 -8.008, 24.915 13.000 -3.446, 19.022 13.000 3.235, 23.528 13.000 -2.144, 31.218 13.000 -6.491, 20.539 13.000 4.188, 23.761 13.000 -2.051, 31.809 13.000 -4.800, 24.992 13.000 -3.207, 22.230 13.000 4.779, 24.010 13.000 -2.020, 24.010 13.000 4.980, 32.010 13.000 -3.020, 25.008 13.000 -2.957, 25.790 13.000 4.779, 24.259 13.000 -2.051, 31.809 13.000 -1.240, 24.961 13.000 -2.711, 31.218 13.000 0.451, 24.854 13.000 -2.484, 27.481 13.000 4.188, 24.492 13.000 -2.144, 30.265 13.000 1.968, 24.695 13.000 -2.291, -10.490 0.500 1.984, 36.010 1.495 5.854, 36.010 0.500 5.980, 36.010 0.500 1.996, 36.010 2.427 5.485, 36.010 3.238 4.896, 36.010 3.877 4.123, 36.010 4.304 3.216, 36.010 4.492 2.231, 36.010 4.429 1.230, 36.010 4.119 0.277, 36.010 3.582 -0.570, 36.010 1.972 -1.739, 36.010 1.001 -1.988, 36.010 -0.972 -1.739, 36.010 -1.851 -1.256, 36.010 -2.582 -0.570, 36.010 -3.119 0.277, 36.010 -3.429 1.230, 36.010 -3.492 2.231, 36.010 -3.304 3.216, 36.010 -2.877 4.123, 36.010 -2.238 4.896, 36.010 -1.427 5.485, 36.010 -0.495 5.854, 24.016 -14.000 -3.000, 20.281 -14.000 -4.472, 20.764 -14.000 -5.351, 21.450 -14.000 -6.082, 22.297 -14.000 -6.619, 23.250 -14.000 -6.929, 24.251 -14.000 -6.992, 25.236 -14.000 -6.804, 26.143 -14.000 -6.377, 26.916 -14.000 -5.738, 27.505 -14.000 -4.927, 24.010 13.000 -3.016 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 2, 1, 3, -1, 4, 5, 6, -1, 4, 6, 7, -1, 3, 5, 4, -1, 8, 9, 3, -1, 8, 10, 9, -1, 8, 4, 10, -1, 8, 3, 4, -1, 11, 12, 13, -1, 11, 14, 12, -1, 15, 14, 11, -1, 15, 11, 16, -1, 16, 11, 17, -1, 18, 19, 20, -1, 21, 17, 22, -1, 18, 23, 19, -1, 18, 24, 23, -1, 25, 26, 3, -1, 18, 27, 24, -1, 18, 9, 11, -1, 18, 11, 27, -1, 18, 20, 9, -1, 28, 3, 26, -1, 29, 25, 3, -1, 16, 17, 21, -1, 30, 3, 28, -1, 31, 29, 3, -1, 32, 3, 30, -1, 33, 31, 3, -1, 34, 33, 3, -1, 9, 35, 34, -1, 9, 36, 35, -1, 9, 37, 36, -1, 9, 20, 37, -1, 17, 38, 39, -1, 17, 40, 38, -1, 17, 41, 40, -1, 17, 32, 41, -1, 9, 34, 3, -1, 17, 3, 32, -1, 22, 17, 39, -1, 11, 13, 27, -1, 7, 6, 17, -1, 17, 11, 42, -1, 11, 43, 42, -1, 43, 7, 42, -1, 7, 17, 42, -1, 44, 45, 46, -1, 47, 48, 49, -1, 48, 50, 49, -1, 51, 47, 49, -1, 50, 46, 49, -1, 45, 51, 49, -1, 46, 45, 49, -1, 11, 10, 43, -1, 9, 10, 11, -1, 10, 52, 53, -1, 54, 4, 55, -1, 10, 56, 52, -1, 10, 57, 56, -1, 10, 58, 57, -1, 59, 60, 61, -1, 62, 10, 63, -1, 64, 58, 10, -1, 64, 10, 65, -1, 66, 61, 60, -1, 65, 10, 4, -1, 67, 63, 10, -1, 67, 68, 69, -1, 67, 70, 68, -1, 71, 60, 59, -1, 67, 53, 70, -1, 67, 69, 63, -1, 72, 4, 54, -1, 67, 10, 53, -1, 73, 66, 60, -1, 74, 4, 72, -1, 75, 60, 71, -1, 76, 60, 75, -1, 77, 73, 60, -1, 65, 4, 74, -1, 78, 77, 60, -1, 63, 79, 78, -1, 63, 80, 79, -1, 63, 81, 80, -1, 63, 69, 81, -1, 4, 82, 55, -1, 4, 83, 82, -1, 4, 76, 83, -1, 4, 60, 76, -1, 63, 78, 60, -1, 30, 84, 32, -1, 85, 84, 30, -1, 86, 30, 28, -1, 86, 85, 30, -1, 87, 28, 26, -1, 87, 86, 28, -1, 88, 26, 25, -1, 88, 87, 26, -1, 29, 88, 25, -1, 89, 88, 29, -1, 90, 29, 31, -1, 90, 89, 29, -1, 91, 31, 33, -1, 91, 90, 31, -1, 92, 33, 34, -1, 92, 91, 33, -1, 93, 34, 35, -1, 93, 92, 34, -1, 36, 93, 35, -1, 94, 93, 36, -1, 95, 94, 36, -1, 95, 36, 37, -1, 96, 95, 37, -1, 96, 37, 20, -1, 19, 96, 20, -1, 97, 96, 19, -1, 98, 19, 23, -1, 98, 97, 19, -1, 99, 23, 24, -1, 99, 98, 23, -1, 100, 24, 27, -1, 100, 99, 24, -1, 101, 27, 13, -1, 101, 100, 27, -1, 102, 13, 12, -1, 102, 101, 13, -1, 103, 12, 14, -1, 103, 102, 12, -1, 15, 103, 14, -1, 104, 103, 15, -1, 105, 15, 16, -1, 105, 104, 15, -1, 106, 16, 21, -1, 106, 105, 16, -1, 107, 21, 22, -1, 107, 106, 21, -1, 108, 22, 39, -1, 108, 107, 22, -1, 109, 39, 38, -1, 109, 108, 39, -1, 110, 38, 40, -1, 110, 109, 38, -1, 111, 40, 41, -1, 111, 110, 40, -1, 32, 111, 41, -1, 84, 111, 32, -1, 43, 10, 62, -1, 43, 112, 7, -1, 113, 43, 62, -1, 113, 112, 43, -1, 60, 114, 115, -1, 114, 60, 116, -1, 60, 115, 117, -1, 116, 60, 118, -1, 60, 117, 119, -1, 120, 121, 122, -1, 121, 123, 122, -1, 123, 124, 122, -1, 124, 125, 122, -1, 125, 126, 122, -1, 126, 127, 122, -1, 118, 60, 128, -1, 127, 129, 122, -1, 129, 130, 122, -1, 130, 128, 122, -1, 60, 119, 131, -1, 60, 132, 63, -1, 60, 131, 133, -1, 128, 60, 122, -1, 132, 134, 135, -1, 134, 136, 135, -1, 136, 137, 135, -1, 137, 138, 135, -1, 138, 139, 135, -1, 139, 140, 135, -1, 140, 120, 135, -1, 122, 141, 135, -1, 141, 63, 135, -1, 120, 122, 135, -1, 63, 132, 135, -1, 60, 133, 132, -1, 47, 142, 143, -1, 142, 144, 143, -1, 47, 51, 142, -1, 145, 146, 147, -1, 146, 44, 147, -1, 148, 145, 149, -1, 150, 148, 149, -1, 151, 150, 149, -1, 142, 152, 153, -1, 154, 151, 149, -1, 155, 154, 149, -1, 156, 155, 149, -1, 144, 156, 149, -1, 147, 143, 149, -1, 143, 144, 149, -1, 145, 147, 149, -1, 142, 153, 144, -1, 157, 158, 146, -1, 159, 157, 146, -1, 145, 159, 146, -1, 45, 44, 146, -1, 160, 51, 45, -1, 161, 45, 162, -1, 162, 45, 163, -1, 51, 164, 165, -1, 164, 51, 166, -1, 163, 45, 167, -1, 51, 165, 168, -1, 166, 51, 169, -1, 51, 168, 170, -1, 169, 51, 160, -1, 171, 172, 173, -1, 174, 175, 176, -1, 177, 174, 142, -1, 170, 177, 142, -1, 51, 170, 142, -1, 174, 176, 142, -1, 178, 173, 146, -1, 158, 178, 146, -1, 179, 167, 146, -1, 172, 179, 146, -1, 142, 176, 180, -1, 173, 172, 146, -1, 167, 45, 146, -1, 142, 180, 152, -1, 181, 160, 45, -1, 182, 181, 45, -1, 161, 182, 45, -1, 75, 183, 76, -1, 184, 183, 75, -1, 185, 75, 71, -1, 185, 184, 75, -1, 186, 71, 59, -1, 186, 185, 71, -1, 187, 59, 61, -1, 187, 186, 59, -1, 66, 187, 61, -1, 188, 187, 66, -1, 189, 66, 73, -1, 189, 188, 66, -1, 190, 73, 77, -1, 190, 189, 73, -1, 191, 77, 78, -1, 191, 190, 77, -1, 192, 78, 79, -1, 192, 191, 78, -1, 80, 192, 79, -1, 193, 192, 80, -1, 194, 193, 80, -1, 194, 80, 81, -1, 195, 194, 81, -1, 195, 81, 69, -1, 68, 195, 69, -1, 196, 195, 68, -1, 197, 68, 70, -1, 197, 196, 68, -1, 198, 70, 53, -1, 198, 197, 70, -1, 199, 53, 52, -1, 199, 198, 53, -1, 200, 52, 56, -1, 200, 199, 52, -1, 201, 56, 57, -1, 201, 200, 56, -1, 202, 57, 58, -1, 202, 201, 57, -1, 64, 202, 58, -1, 203, 202, 64, -1, 204, 64, 65, -1, 204, 203, 64, -1, 205, 65, 74, -1, 205, 204, 65, -1, 206, 74, 72, -1, 206, 205, 74, -1, 207, 72, 54, -1, 207, 206, 72, -1, 208, 54, 55, -1, 208, 207, 54, -1, 209, 55, 82, -1, 209, 208, 55, -1, 210, 82, 83, -1, 210, 209, 82, -1, 76, 210, 83, -1, 183, 210, 76, -1, 62, 211, 141, -1, 62, 141, 212, -1, 213, 214, 215, -1, 216, 100, 214, -1, 216, 214, 213, -1, 217, 215, 218, -1, 217, 218, 219, -1, 217, 213, 215, -1, 220, 99, 100, -1, 220, 100, 216, -1, 221, 219, 222, -1, 221, 217, 219, -1, 223, 98, 99, -1, 223, 99, 220, -1, 224, 222, 225, -1, 224, 221, 222, -1, 226, 227, 98, -1, 226, 98, 223, -1, 228, 225, 229, -1, 228, 224, 225, -1, 230, 231, 227, -1, 230, 227, 226, -1, 232, 228, 229, -1, 232, 229, 233, -1, 234, 235, 231, -1, 234, 231, 230, -1, 236, 232, 233, -1, 237, 235, 234, -1, 238, 237, 239, -1, 238, 235, 237, -1, 240, 236, 233, -1, 240, 241, 236, -1, 242, 238, 239, -1, 110, 241, 240, -1, 110, 243, 241, -1, 244, 239, 245, -1, 244, 242, 239, -1, 111, 243, 110, -1, 111, 246, 243, -1, 247, 245, 248, -1, 247, 244, 245, -1, 84, 246, 111, -1, 249, 248, 250, -1, 249, 247, 248, -1, 251, 252, 246, -1, 251, 246, 84, -1, 253, 250, 254, -1, 253, 249, 250, -1, 255, 256, 252, -1, 255, 252, 251, -1, 257, 254, 258, -1, 257, 253, 254, -1, 259, 258, 256, -1, 259, 257, 258, -1, 259, 256, 255, -1, 118, 260, 261, -1, 118, 128, 260, -1, 116, 261, 262, -1, 116, 118, 261, -1, 114, 262, 263, -1, 114, 116, 262, -1, 115, 263, 264, -1, 115, 114, 263, -1, 117, 264, 265, -1, 117, 115, 264, -1, 119, 265, 266, -1, 119, 117, 265, -1, 131, 266, 267, -1, 131, 119, 266, -1, 133, 267, 268, -1, 133, 131, 267, -1, 132, 268, 269, -1, 132, 133, 268, -1, 134, 269, 270, -1, 134, 132, 269, -1, 136, 270, 271, -1, 136, 134, 270, -1, 137, 136, 271, -1, 137, 271, 272, -1, 138, 137, 272, -1, 138, 272, 273, -1, 139, 138, 273, -1, 139, 273, 274, -1, 140, 139, 274, -1, 140, 274, 275, -1, 120, 275, 276, -1, 120, 140, 275, -1, 121, 276, 277, -1, 121, 120, 276, -1, 123, 277, 278, -1, 123, 121, 277, -1, 124, 278, 279, -1, 124, 123, 278, -1, 125, 279, 280, -1, 125, 124, 279, -1, 281, 125, 280, -1, 126, 125, 281, -1, 127, 281, 282, -1, 127, 126, 281, -1, 283, 127, 282, -1, 129, 127, 283, -1, 130, 283, 284, -1, 130, 129, 283, -1, 128, 284, 260, -1, 128, 130, 284, -1, 285, 152, 180, -1, 285, 180, 286, -1, 286, 180, 176, -1, 286, 176, 287, -1, 287, 176, 175, -1, 287, 175, 288, -1, 288, 175, 289, -1, 288, 289, 290, -1, 290, 289, 291, -1, 290, 291, 292, -1, 292, 291, 293, -1, 292, 293, 294, -1, 294, 293, 295, -1, 294, 295, 296, -1, 296, 295, 297, -1, 296, 297, 298, -1, 298, 297, 299, -1, 298, 299, 300, -1, 300, 299, 171, -1, 300, 171, 301, -1, 301, 171, 173, -1, 301, 173, 302, -1, 302, 173, 178, -1, 302, 178, 303, -1, 303, 178, 158, -1, 303, 158, 304, -1, 304, 158, 157, -1, 304, 157, 305, -1, 305, 157, 159, -1, 305, 159, 306, -1, 306, 159, 145, -1, 306, 145, 307, -1, 307, 145, 148, -1, 307, 148, 308, -1, 308, 148, 150, -1, 308, 150, 309, -1, 309, 150, 151, -1, 309, 151, 310, -1, 310, 151, 154, -1, 310, 154, 311, -1, 311, 154, 155, -1, 311, 155, 312, -1, 312, 155, 156, -1, 312, 156, 313, -1, 313, 156, 144, -1, 313, 144, 314, -1, 314, 144, 153, -1, 314, 153, 315, -1, 285, 315, 152, -1, 315, 153, 152, -1, 291, 177, 170, -1, 291, 170, 168, -1, 179, 172, 299, -1, 172, 171, 299, -1, 167, 179, 297, -1, 293, 291, 316, -1, 163, 167, 297, -1, 295, 293, 316, -1, 297, 295, 316, -1, 179, 299, 297, -1, 168, 165, 316, -1, 165, 164, 316, -1, 164, 166, 316, -1, 166, 169, 316, -1, 169, 160, 316, -1, 160, 181, 316, -1, 181, 182, 316, -1, 182, 161, 316, -1, 161, 162, 316, -1, 162, 163, 316, -1, 163, 297, 316, -1, 291, 168, 316, -1, 289, 175, 174, -1, 291, 289, 177, -1, 289, 174, 177, -1, 317, 318, 319, -1, 320, 321, 318, -1, 320, 318, 317, -1, 322, 319, 323, -1, 322, 323, 324, -1, 322, 317, 319, -1, 325, 326, 321, -1, 325, 321, 320, -1, 327, 324, 328, -1, 327, 322, 324, -1, 329, 330, 326, -1, 329, 326, 325, -1, 331, 328, 332, -1, 331, 327, 328, -1, 333, 334, 330, -1, 333, 330, 329, -1, 335, 332, 336, -1, 335, 331, 332, -1, 337, 338, 334, -1, 337, 334, 333, -1, 339, 335, 336, -1, 339, 336, 340, -1, 341, 194, 338, -1, 341, 338, 337, -1, 342, 339, 340, -1, 343, 194, 341, -1, 344, 343, 345, -1, 344, 194, 343, -1, 346, 342, 340, -1, 346, 347, 342, -1, 348, 344, 345, -1, 349, 347, 346, -1, 349, 350, 347, -1, 351, 345, 352, -1, 351, 348, 345, -1, 353, 350, 349, -1, 353, 354, 350, -1, 355, 354, 353, -1, 356, 352, 357, -1, 356, 351, 352, -1, 358, 359, 354, -1, 358, 354, 355, -1, 360, 357, 361, -1, 360, 356, 357, -1, 362, 361, 363, -1, 362, 360, 361, -1, 364, 365, 359, -1, 364, 359, 358, -1, 366, 363, 367, -1, 366, 362, 363, -1, 186, 367, 365, -1, 186, 366, 367, -1, 186, 365, 364, -1, 368, 246, 252, -1, 368, 252, 256, -1, 368, 256, 258, -1, 368, 258, 254, -1, 368, 254, 250, -1, 368, 250, 248, -1, 368, 248, 245, -1, 368, 245, 239, -1, 368, 239, 237, -1, 368, 237, 234, -1, 368, 234, 230, -1, 368, 230, 226, -1, 368, 226, 223, -1, 368, 223, 220, -1, 368, 220, 216, -1, 368, 216, 213, -1, 368, 213, 217, -1, 368, 217, 221, -1, 368, 221, 224, -1, 368, 224, 228, -1, 368, 228, 232, -1, 368, 232, 236, -1, 368, 236, 241, -1, 368, 241, 243, -1, 368, 243, 246, -1, 369, 370, 371, -1, 372, 369, 371, -1, 373, 372, 371, -1, 374, 373, 371, -1, 375, 374, 371, -1, 376, 375, 371, -1, 377, 376, 371, -1, 378, 377, 371, -1, 379, 378, 371, -1, 270, 379, 371, -1, 380, 270, 371, -1, 381, 380, 371, -1, 273, 381, 371, -1, 382, 273, 371, -1, 383, 382, 371, -1, 384, 383, 371, -1, 385, 384, 371, -1, 386, 385, 371, -1, 387, 386, 371, -1, 388, 387, 371, -1, 389, 388, 371, -1, 390, 389, 371, -1, 391, 390, 371, -1, 392, 391, 371, -1, 370, 392, 371, -1, 393, 285, 286, -1, 393, 286, 287, -1, 393, 287, 288, -1, 393, 288, 290, -1, 393, 290, 292, -1, 393, 292, 294, -1, 393, 294, 296, -1, 393, 296, 298, -1, 393, 298, 300, -1, 393, 300, 301, -1, 393, 301, 302, -1, 393, 302, 303, -1, 393, 303, 304, -1, 393, 304, 394, -1, 393, 394, 395, -1, 393, 395, 396, -1, 393, 396, 397, -1, 393, 397, 398, -1, 393, 398, 399, -1, 393, 399, 400, -1, 393, 400, 401, -1, 393, 401, 402, -1, 393, 402, 403, -1, 393, 403, 315, -1, 393, 315, 285, -1, 404, 354, 359, -1, 404, 359, 365, -1, 404, 365, 367, -1, 404, 367, 363, -1, 404, 363, 361, -1, 404, 361, 357, -1, 404, 357, 352, -1, 404, 352, 345, -1, 404, 345, 343, -1, 404, 343, 341, -1, 404, 341, 337, -1, 404, 337, 333, -1, 404, 333, 329, -1, 404, 329, 325, -1, 404, 325, 320, -1, 404, 320, 317, -1, 404, 317, 322, -1, 404, 322, 327, -1, 404, 327, 331, -1, 404, 331, 335, -1, 404, 335, 339, -1, 404, 339, 342, -1, 404, 342, 347, -1, 404, 347, 350, -1, 404, 350, 354, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 12.760 -0.500 59.329 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 12.760 -0.500 -78.369 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 12.760 -69.349 -9.520 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 12.760 68.349 -9.520 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -56.089 -0.500 -9.520 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 81.609 -0.500 -9.520 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_HIP_Y.wrl000066400000000000000000002117121207742442300227600ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 63.000 41.997 37.000 center 0.000 0.000 0.000 #translation -10.500 -0.001 20.500 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.50 0.50 0.50 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -14.000 2.000 -32.000, -14.000 0.956 -34.932, -14.000 1.132 -31.924, -14.000 -0.071 -34.727, -14.000 5.830 -30.214, -14.000 7.657 -32.657, -14.000 5.214 -30.830, -14.000 6.870 -33.347, -14.000 4.500 -31.330, -14.000 6.000 -33.928, -14.000 -3.000 -27.000, -14.000 -2.924 -27.868, -14.000 -4.000 -27.000, -14.000 -5.727 -29.071, -14.000 3.710 -31.698, -14.000 5.061 -34.391, -14.000 4.071 -34.727, -14.000 0.290 -31.698, -14.000 -1.061 -34.391, -14.000 2.868 -31.924, -14.000 3.044 -34.932, -11.500 3.044 -34.932, -11.500 2.868 -31.924, -11.500 2.000 -32.000, -11.500 4.071 -34.727, -11.500 3.710 -31.698, -11.500 -0.071 -34.727, -11.500 1.132 -31.924, -11.500 0.290 -31.698, -11.500 -1.061 -34.391, -11.500 9.664 -24.706, -11.500 6.719 -25.348, -11.500 6.929 -26.162, -11.500 9.916 -25.841, -11.500 0.956 -34.932, -11.500 2.000 -35.000, -14.000 2.000 -35.000, -14.000 0.381 -19.166, -14.000 1.476 -22.028, -14.000 0.663 -22.182, -14.000 -0.613 -19.439, 39.500 5.830 -30.214, 39.500 10.485 -35.485, 39.500 5.214 -30.830, 39.500 9.482 -36.382, 39.500 8.384 -37.161, 39.500 4.500 -31.330, 39.500 7.207 -37.812, 39.500 3.710 -31.698, 39.500 5.963 -38.327, 39.500 4.670 -38.699, 39.500 2.868 -31.924, 39.500 3.344 -38.925, 39.500 2.000 -32.000, 39.500 2.000 -39.000, 39.500 0.689 -38.928, 39.500 1.159 -31.929, 39.500 -0.606 -38.714, -11.500 5.061 -34.391, -11.500 6.000 -33.928, -11.500 6.870 -33.347, -11.500 7.657 -32.657, -14.000 8.347 -31.870, -11.500 8.347 -31.870, -14.000 8.928 -31.000, -11.500 8.928 -31.000, -14.000 9.391 -30.061, -14.000 9.727 -29.071, -11.500 9.391 -30.061, -11.500 9.727 -29.071, -14.000 9.932 -28.044, -11.500 9.932 -28.044, -14.000 10.000 -27.000, -11.500 7.976 -21.682, -11.500 8.684 -22.604, -11.500 7.595 -20.661, -11.500 7.142 -20.872, -11.500 6.312 -20.262, -11.500 5.411 -19.763, -11.500 3.556 -12.000, -11.500 3.454 -19.133, -11.500 4.453 -19.385, -11.500 1.797 -16.854, -11.500 2.431 -19.012, -11.500 1.401 -19.022, -11.500 0.381 -19.166, -11.500 -0.613 -19.439, -14.000 7.976 -21.682, -14.000 7.595 -20.661, -14.000 8.684 -22.604, -14.000 7.142 -20.872, -14.000 3.454 -19.133, -14.000 1.797 -16.854, -14.000 4.453 -19.385, -14.000 2.431 -19.012, -14.000 1.401 -19.022, -14.000 3.556 -12.000, -14.000 -5.803 -17.777, -14.000 -4.000 -21.708, -14.000 -4.700 -22.628, -14.000 -5.260 -23.639, -14.000 -5.668 -24.719, -14.000 -4.000 -12.000, -14.000 -5.145 -12.000, 24.574 14.000 -5.000, 18.796 9.365 -5.000, 17.380 11.788 -5.000, 15.652 14.000 -5.000, 25.326 8.862 -5.000, 30.081 7.249 -5.000, 26.530 5.361 -5.000, 19.877 6.775 -5.000, 42.000 -0.802 -22.859, 42.000 -1.500 -15.522, 42.000 -1.500 -23.429, 42.000 -0.013 -22.423, 42.000 0.841 -22.136, 42.000 12.236 -20.737, 42.000 6.250 -21.003, 42.000 6.285 -24.423, 42.000 12.861 -21.897, 42.000 3.344 -38.925, 42.000 2.000 -39.000, 42.000 4.670 -38.699, 42.000 5.963 -38.327, 42.000 7.207 -37.812, 42.000 8.384 -37.161, 42.000 9.482 -36.382, 42.000 10.485 -35.485, 42.000 11.382 -34.482, 39.500 11.382 -34.482, 42.000 12.161 -33.384, 39.500 12.161 -33.384, 42.000 12.812 -32.207, 39.500 12.812 -32.207, 42.000 13.327 -30.963, 39.500 13.327 -30.963, 42.000 13.699 -29.670, 39.500 13.699 -29.670, 42.000 13.925 -28.344, 39.500 13.925 -28.344, 39.500 14.000 -27.000, 17.286 -3.750 -5.000, 20.946 -1.500 -5.000, 17.126 -6.000 -5.000, 13.626 -1.500 -5.000, 13.305 -1.671 -5.000, 8.145 16.463 -5.000, 14.834 14.865 -5.000, 13.967 15.682 -5.000, 13.056 16.448 -5.000, 6.577 17.826 -5.000, 0.638 14.000 -5.000, 6.577 17.826 -12.000, 4.980 18.336 -5.000, 3.962 18.582 -12.000, -14.000 -12.845 -8.409, -14.000 -12.845 -12.000, -14.000 -12.995 -12.000, -14.000 -14.249 -8.500, 4.934 6.250 -5.000, -0.000 11.000 -5.000, 1.226 3.807 -5.000, 42.000 3.713 -15.123, 42.000 -1.500 -12.000, 42.000 2.401 -15.007, 42.000 1.085 -15.035, 42.000 -0.221 -15.207, 42.000 14.000 -12.000, 42.000 14.000 -27.000, 39.500 14.000 -12.000, -0.094 19.972 -12.000, -1.455 18.944 -12.000, -0.056 21.000 -12.000, 1.267 18.958 -12.000, 1.214 19.979 -12.000, 2.483 20.853 -12.000, -11.500 11.000 -12.000, -14.000 11.000 -12.000, -11.500 12.000 -12.000, -11.500 10.000 -12.000, -14.000 10.000 -12.000, -14.000 12.000 -12.000, -14.000 12.845 -12.000, -11.500 15.124 -12.000, -14.000 15.652 -12.000, -12.005 17.230 -12.000, 4.986 20.400 -12.000, -9.221 16.612 -12.000, -9.834 18.555 -12.000, -6.753 17.759 -12.000, -7.519 19.608 -12.000, 7.415 19.647 -12.000, 33.984 12.000 -12.000, 33.984 14.000 -12.000, 39.000 12.000 -12.000, 35.000 14.000 -12.000, -4.146 18.542 -12.000, -5.094 20.373 -12.000, -2.594 20.839 -12.000, -11.000 14.000 -5.000, -11.000 -1.500 -5.000, -11.500 11.000 -5.000, -11.500 10.000 -5.000, -11.500 4.556 -5.000, -11.500 3.556 -5.000, -11.500 -15.000 -5.000, -9.583 -16.406 -5.000, -11.662 -15.000 -5.000, -11.500 -12.995 -5.000, -11.500 -11.995 -5.000, -3.708 -1.500 -5.000, -11.500 -4.000 -5.000, -3.189 -2.415 -5.000, -14.000 -15.652 -5.000, -14.000 -15.652 -12.000, -11.950 -17.269 -12.000, -11.950 -17.269 -5.000, 7.415 19.647 -5.000, 4.676 20.473 -5.000, 18.994 0.487 -2.000, 23.664 3.970 -2.000, 18.975 0.974 -2.000, 18.944 1.460 -2.000, 26.518 0.235 -2.000, 32.265 2.142 -2.000, -17.928 6.291 -2.000, -16.919 8.646 -2.000, -19.170 8.573 -2.000, -17.784 11.168 -2.000, 12.836 -1.788 -2.000, 18.809 2.690 -2.000, 18.594 3.908 -2.000, 18.300 5.110 -2.000, 2.789 -2.867 -2.000, 1.936 -3.500 -2.000, 13.746 -3.500 -2.000, 3.446 -2.032 -2.000, 34.361 -1.800 -2.000, 34.776 -1.580 -2.000, 34.733 -1.500 -2.000, 34.235 1.723 -2.000, 35.000 -1.500 -2.000, 39.000 -1.500 -2.000, 39.000 12.000 -2.000, 42.000 -1.500 -2.000, 42.000 14.000 -2.000, 39.500 -1.500 -2.000, 39.500 14.000 -2.000, 35.000 14.000 -2.000, 33.984 12.000 -2.000, 33.984 14.000 -2.000, 7.415 19.647 -2.000, 6.577 17.826 -2.000, 4.986 20.400 -2.000, 3.962 18.582 -2.000, 2.483 20.853 -2.000, 1.267 18.958 -2.000, 1.214 19.979 -2.000, -0.056 21.000 -2.000, -0.094 19.972 -2.000, -1.455 18.944 -2.000, -2.594 20.839 -2.000, -4.146 18.542 -2.000, -5.094 20.373 -2.000, -6.753 17.759 -2.000, -7.519 19.608 -2.000, -9.834 18.555 -2.000, -9.221 16.612 -2.000, -12.005 17.230 -2.000, -11.500 15.124 -2.000, -14.000 15.652 -2.000, -14.000 -15.652 -2.000, -11.950 -17.269 -2.000, -11.662 -15.000 -2.000, -14.000 -12.845 -2.000, -11.500 -15.000 -2.000, -11.500 -5.145 -2.000, -14.000 -5.145 -2.000, -14.000 -3.500 -2.000, -11.500 -3.500 -2.000, -11.500 -3.000 -2.000, -14.000 -3.000 -2.000, -11.500 1.292 -2.000, -14.000 1.292 -2.000, -14.000 11.000 -2.000, -11.500 11.000 -2.000, -14.000 12.000 -2.000, -11.500 12.000 -2.000, -14.000 12.845 -2.000, -9.563 -16.418 -2.000, 14.515 -15.176 -2.000, 16.286 -13.257 -2.000, 13.722 -13.142 -2.000, 11.851 -14.851 -2.000, 12.520 -16.860 -2.000, 15.348 -11.200 -2.000, 17.805 -11.134 -2.000, 9.770 -16.296 -2.000, 10.331 -18.283 -2.000, 16.702 -9.058 -2.000, 7.515 -17.451 -2.000, 7.983 -19.424 -2.000, 5.127 -18.295 -2.000, 5.511 -20.264 -2.000, 2.647 -18.815 -2.000, 2.954 -20.791 -2.000, 0.351 -20.997 -2.000, 0.120 -19.000 -2.000, -2.257 -20.878 -2.000, -2.408 -18.847 -2.000, -4.830 -20.437 -2.000, -4.894 -18.359 -2.000, -7.329 -19.680 -2.000, -7.293 -17.544 -2.000, -9.714 -18.618 -2.000, -8.504 -17.518 -2.000, 33.984 14.000 -5.000, 35.000 14.000 -5.000, 39.500 14.000 -5.000, 42.000 14.000 -5.000, 42.000 -1.500 -5.000, 39.500 -1.500 -5.000, 39.000 -1.500 -5.000, 38.125 -1.500 -3.500, 37.250 -1.500 -3.500, 36.125 -1.500 -3.500, 35.000 -1.500 -5.000, 36.314 11.505 -2.000, 35.633 5.100 -2.000, 8.890 16.792 -2.000, 19.864 12.955 -2.000, 14.731 12.000 -2.000, 13.003 13.853 -2.000, 11.045 15.460 -2.000, 2.079 18.781 -2.000, -2.462 15.479 -2.000, -0.191 15.391 -2.000, 0.638 12.000 -2.000, -3.432 -2.055 -2.000, -2.780 -2.876 -2.000, -1.936 -3.500 -2.000, -3.848 -1.092 -2.000, -4.000 -0.055 -2.000, -3.876 0.987 -2.000, -0.000 9.000 -2.000, -2.032 3.446 -2.000, 3.722 4.250 -2.000, 1.013 3.870 -2.000, 3.460 2.008 -2.000, 3.863 1.040 -2.000, 2.819 2.838 -2.000, 1.984 3.473 -2.000, -0.027 4.000 -2.000, -1.066 3.855 -2.000, -3.487 1.960 -2.000, 4.000 -0.000 -2.000, 3.859 -1.053 -2.000, -2.857 2.799 -2.000, 2.520 -11.250 -2.000, -1.000 -3.873 -2.000, 0.000 -4.000 -2.000, 1.000 -3.873 -2.000, 17.963 -10.877 -2.000, 18.118 -10.619 -2.000, 18.268 -10.357 -2.000, 17.551 -7.277 -2.000, 23.109 -4.728 -2.000, 31.165 -3.500 -2.000, 18.675 -3.500 -2.000, 18.211 -5.418 -2.000, 1.850 20.918 -5.000, -1.010 20.976 -5.000, -3.851 20.644 -5.000, -6.621 19.929 -5.000, -9.268 18.844 -5.000, -11.743 17.410 -5.000, -14.000 15.652 -5.000, -15.903 13.715 -5.000, -16.050 13.543 -2.000, -17.539 11.548 -5.000, -18.883 9.189 -5.000, -19.911 6.675 -5.000, -20.180 5.811 -2.000, -20.606 4.050 -5.000, -20.794 2.934 -2.000, -20.956 1.358 -5.000, -21.000 -0.000 -2.000, -20.956 -1.358 -5.000, -20.794 -2.934 -2.000, -20.606 -4.050 -5.000, -20.180 -5.811 -2.000, -19.911 -6.675 -5.000, -19.170 -8.573 -2.000, -18.883 -9.189 -5.000, -17.784 -11.168 -2.000, -17.539 -11.548 -5.000, -16.050 -13.543 -2.000, -15.903 -13.715 -5.000, -9.714 -18.618 -5.000, -7.329 -19.680 -5.000, -4.830 -20.437 -5.000, -2.257 -20.878 -5.000, 0.351 -20.997 -5.000, 2.954 -20.791 -5.000, 5.511 -20.264 -5.000, 7.983 -19.424 -5.000, 10.331 -18.283 -5.000, 12.520 -16.860 -5.000, 14.515 -15.176 -5.000, 16.286 -13.257 -5.000, 17.805 -11.134 -5.000, 17.963 -10.877 -5.000, 18.118 -10.619 -5.000, 18.268 -10.357 -5.000, -15.601 10.844 -2.000, -15.601 -10.844 -2.000, -16.919 -8.646 -2.000, -19.054 -7.192 -2.000, -17.928 -6.291 -2.000, -19.703 -4.372 -2.000, -18.612 -3.821 -2.000, -19.978 -1.467 -2.000, -18.957 -1.281 -2.000, -19.978 1.467 -2.000, -18.957 1.281 -2.000, -19.978 -0.000 -2.000, -18.612 3.821 -2.000, -19.703 4.372 -2.000, -19.054 7.192 -2.000, -16.478 -0.000 -2.000, -17.718 -0.000 -2.000, -16.306 -1.910 -2.000, 39.500 -1.500 -12.000, 34.733 -1.500 -5.000, 39.000 -1.500 -12.000, 37.000 5.250 -5.000, 35.000 7.321 -5.000, 39.000 12.000 -5.000, 35.994 12.000 -5.000, 34.776 -1.580 -5.000, 26.522 -5.969 -3.500, 18.855 -2.341 -2.000, 18.964 -1.173 -2.000, 19.000 0.000 -2.000, 29.612 7.132 -2.000, 32.963 6.614 -2.000, 25.523 6.862 -2.000, 17.430 7.563 -2.000, 16.233 9.874 -2.000, 7.684 15.391 -2.000, 7.733 16.608 -2.000, 3.874 0.997 -5.000, 4.000 -0.000 -5.000, 3.503 1.931 -5.000, 2.912 2.743 -5.000, 2.136 3.382 -5.000, 0.238 3.993 -5.000, -0.764 3.926 -5.000, -1.718 3.612 -5.000, -2.564 3.070 -5.000, -3.248 2.334 -5.000, -3.728 1.451 -5.000, -3.972 0.476 -5.000, -3.965 -0.529 -5.000, -2.450 -3.162 -5.000, -1.541 -3.691 -5.000, -0.526 -3.965 -5.000, 0.526 -3.965 -5.000, 1.541 -3.691 -5.000, 2.450 -3.162 -5.000, 3.189 -2.415 -5.000, 3.708 -1.500 -5.000, 3.869 -1.014 -5.000, 3.967 -0.511 -5.000, 15.918 -1.020 -2.000, 20.982 3.285 -2.000, 9.150 8.555 -2.000, 16.211 -6.279 -2.000, 17.211 -6.348 -2.000, 16.211 -4.890 -2.000, -14.000 10.000 -5.000, -14.000 4.556 -5.000, -14.000 11.000 -5.000, -15.770 12.358 -5.000, -14.000 -15.000 -5.000, -14.000 12.845 -5.000, -14.000 15.000 -5.000, -14.000 15.500 -5.000, -14.000 -12.995 -5.000, -17.478 -0.000 -5.000, -14.000 -12.845 -5.000, -17.303 -2.025 -5.000, -14.000 -3.000 -5.000, -15.739 0.278 -5.000, -14.000 3.556 -5.000, -17.303 2.025 -5.000, -14.000 -11.995 -5.000, -15.770 -12.632 -5.000, -14.000 -4.000 -5.000, 12.520 -16.860 -12.000, 10.331 -18.283 -12.000, 14.515 -15.176 -12.000, -9.714 -18.618 -12.000, 16.286 -13.257 -12.000, -7.329 -19.680 -12.000, 17.805 -11.134 -12.000, -4.830 -20.437 -12.000, -2.257 -20.878 -12.000, 0.351 -20.997 -12.000, 2.954 -20.791 -12.000, 5.511 -20.264 -12.000, 7.983 -19.424 -12.000, -4.964 -18.340 -5.000, -7.338 -17.526 -5.000, 18.000 -10.500 -5.000, -2.504 -18.834 -5.000, 0.000 -19.000 -5.000, 16.702 -9.058 -5.000, 2.543 -18.829 -5.000, 5.040 -18.319 -5.000, 7.446 -17.480 -5.000, 9.718 -16.326 -5.000, 13.701 -13.164 -5.000, 15.339 -11.212 -5.000, -11.500 -3.000 -5.000, 11.816 -14.879 -5.000, -11.500 15.000 -5.000, -11.500 15.124 -5.000, -9.148 16.653 -5.000, -6.596 17.818 -5.000, -3.901 18.595 -5.000, -1.120 18.967 -5.000, 1.685 18.925 -5.000, 42.000 13.928 -25.685, 42.000 13.712 -24.386, 42.000 13.355 -23.118, 42.000 11.488 -19.653, 42.000 10.626 -18.658, 42.000 9.660 -17.763, 42.000 8.601 -16.979, 42.000 7.463 -16.316, 42.000 6.260 -15.781, 42.000 5.005 -15.382, 39.500 -1.500 -15.522, 34.235 3.723 -5.000, 31.666 4.269 -5.000, 35.000 12.000 -5.000, 36.492 12.000 -10.250, 36.492 12.000 -8.500, 35.238 12.000 -8.500, 33.984 12.000 -5.000, 37.746 12.000 -8.500, 24.981 -3.071 -5.000, 19.596 -7.549 -5.000, 20.496 -4.574 -5.000, 27.965 4.637 -2.000, 20.868 2.351 -5.000, 20.644 3.848 -5.000, 20.314 5.325 -5.000, -14.000 14.249 -8.500, -14.000 -12.845 -6.824, -14.000 -13.923 -6.750, -14.000 -13.452 -8.126, -14.000 11.923 -8.500, -14.000 6.146 -8.500, -14.000 1.292 -12.000, -14.000 4.556 -12.000, -14.000 8.073 -8.500, -14.000 0.778 -8.500, -14.000 -1.111 -8.500, -14.000 -3.000 -12.000, -14.000 -3.500 -12.000, -14.000 -7.997 -6.750, -14.000 -7.923 -8.500, -14.000 -5.961 -8.500, -11.662 -15.000 -12.000, -11.639 -12.995 -12.000, 0.120 -19.000 -12.000, 2.647 -18.815 -12.000, -2.408 -18.847 -12.000, -4.894 -18.359 -12.000, 13.722 -13.142 -12.000, -7.293 -17.544 -12.000, 11.851 -14.851 -12.000, 15.348 -11.200 -12.000, -8.504 -17.518 -12.000, -9.563 -16.418 -12.000, 9.770 -16.296 -12.000, 16.702 -9.058 -12.000, 7.515 -17.451 -12.000, 5.127 -18.295 -12.000, -11.500 -15.000 -12.000, -11.500 -12.995 -12.000, -11.500 -13.452 -8.126, -11.500 -5.145 -12.000, -11.500 -11.995 -12.000, -11.500 -12.845 -12.000, -11.500 -3.500 -12.000, -11.500 -3.000 -12.000, -11.500 -4.000 -12.000, -11.500 -8.226 -8.500, -11.500 -7.997 -6.750, -11.500 -6.113 -8.500, -11.500 -1.111 -8.500, -11.500 0.778 -8.500, -11.500 1.292 -12.000, -11.500 6.146 -8.500, -11.500 4.556 -12.000, -11.500 8.073 -8.500, -11.500 12.031 -8.500, -11.500 13.062 -8.500, -11.500 13.000 -6.750, -11.500 14.093 -8.500, 3.345 18.703 -5.000, 39.500 -0.221 -15.207, 39.500 3.713 -15.123, 39.500 2.401 -15.007, 39.500 1.085 -15.035, 39.500 13.928 -25.685, 39.500 13.712 -24.386, 39.500 13.355 -23.118, 39.500 12.861 -21.897, 39.500 12.236 -20.737, 39.500 11.488 -19.653, 39.500 10.626 -18.658, 39.500 9.660 -17.763, 39.500 8.601 -16.979, 39.500 7.463 -16.316, 39.500 6.260 -15.781, 39.500 5.005 -15.382, 20.280 14.913 -8.500, -11.500 10.000 -27.000, 42.000 2.633 -22.040, 42.000 6.678 -25.234, 42.000 1.733 -22.007, 42.000 6.919 -26.103, 42.000 7.000 -27.000, 42.000 5.753 -23.696, 42.000 5.099 -23.076, 42.000 4.344 -22.583, 42.000 3.513 -22.234, 39.500 -2.733 -15.973, 42.000 -2.733 -15.973, 42.000 -3.909 -16.556, 39.500 -3.909 -16.556, 39.500 -5.014 -17.264, 42.000 -5.014 -17.264, 42.000 -6.036 -18.088, 39.500 -6.036 -18.088, 39.500 -6.961 -19.019, 42.000 -6.961 -19.019, 42.000 -7.779 -20.046, 39.500 -7.779 -20.046, 39.500 -8.481 -21.156, 42.000 -8.481 -21.156, 39.500 -9.056 -22.335, 42.000 -9.056 -22.335, 39.500 -9.500 -23.571, 42.000 -9.500 -23.571, 39.500 -9.805 -24.847, 42.000 -9.805 -24.847, 39.500 -9.970 -26.150, 42.000 -9.970 -26.150, 39.500 -9.991 -27.462, 42.000 -9.991 -27.462, 39.500 -9.869 -28.769, 42.000 -9.869 -28.769, 39.500 -9.605 -30.055, 42.000 -9.605 -30.055, 39.500 -9.201 -31.304, 42.000 -9.201 -31.304, 39.500 -8.664 -32.502, 42.000 -8.664 -32.502, 39.500 -7.999 -33.634, 42.000 -7.999 -33.634, 39.500 -7.215 -34.687, 42.000 -7.215 -34.687, 42.000 -6.320 -35.647, 39.500 -6.320 -35.647, 39.500 -5.326 -36.504, 42.000 -5.326 -36.504, 39.500 -4.244 -37.248, 42.000 -4.244 -37.248, 39.500 -3.087 -37.868, 42.000 -3.087 -37.868, 39.500 -1.870 -38.359, 42.000 -1.870 -38.359, 42.000 -0.606 -38.714, 42.000 0.689 -38.928, 27.801 1.930 -5.000, 20.976 -1.000 -5.000, 20.994 -0.500 -5.000, 21.000 0.000 -5.000, 20.985 0.785 -5.000, 20.941 1.569 -5.000, 9.939 10.388 -5.000, 23.203 4.563 -5.000, -14.000 -11.995 -12.000, -14.000 -6.000 -27.000, -14.000 -5.917 -25.848, -14.000 -3.270 -20.981, -14.000 -1.562 -19.837, -14.000 -2.453 -20.354, -14.000 5.411 -19.763, -14.000 6.312 -20.262, -14.000 9.664 -24.706, -14.000 9.916 -25.841, -14.000 9.250 -23.619, -12.750 -9.497 -19.500, -11.500 -6.000 -27.000, -11.500 -5.917 -25.848, -11.500 -5.668 -24.719, -11.500 -5.803 -17.777, -11.500 -4.700 -22.628, -11.500 -4.000 -21.708, -11.500 -5.260 -23.639, -11.500 -3.270 -20.981, -11.500 -1.562 -19.837, -11.500 -2.453 -20.354, -11.500 9.250 -23.619, 18.622 14.317 -5.000, 39.500 6.250 -21.003, 39.500 2.633 -22.040, 39.500 5.753 -23.696, 39.500 5.099 -23.076, 39.500 4.344 -22.583, 39.500 3.513 -22.234, 39.500 6.285 -24.423, 39.500 0.841 -22.136, 39.500 -0.013 -22.423, 39.500 -0.802 -22.859, 39.500 -1.500 -23.429, 39.500 1.733 -22.007, 39.500 6.678 -25.234, 39.500 6.919 -26.103, 39.500 7.000 -27.000, 42.000 -2.051 -24.069, 42.000 -2.486 -24.792, 42.000 -2.794 -25.578, 42.000 -2.964 -26.405, 42.000 -2.994 -27.248, 42.000 -2.881 -28.085, 42.000 -2.629 -28.890, 42.000 -2.245 -29.642, 42.000 -1.740 -30.318, 42.000 -1.129 -30.900, 42.000 -0.428 -31.371, 42.000 0.342 -31.717, 42.000 1.159 -31.929, 42.000 2.000 -32.000, 42.000 6.924 -27.868, 42.000 2.868 -31.924, 42.000 6.698 -28.710, 42.000 3.710 -31.698, 42.000 6.330 -29.500, 42.000 4.500 -31.330, 42.000 5.830 -30.214, 42.000 5.214 -30.830, 39.500 -2.051 -24.069, 39.500 -2.486 -24.792, 39.500 -2.794 -25.578, 39.500 -2.964 -26.405, 39.500 -2.994 -27.248, 39.500 -2.881 -28.085, 39.500 -2.629 -28.890, 39.500 -2.245 -29.642, 39.500 -1.740 -30.318, 39.500 -1.129 -30.900, 39.500 -0.428 -31.371, 39.500 0.342 -31.717, 39.500 6.924 -27.868, 39.500 6.698 -28.710, 39.500 6.330 -29.500, 29.098 5.759 -5.000, -14.000 -4.000 -26.067, -14.000 -5.000 -24.354, -14.000 2.303 -22.009, -14.000 3.122 -22.127, -14.000 3.910 -22.379, -14.000 -1.472 -23.402, -14.000 -2.018 -24.024, -14.000 -0.831 -22.879, -14.000 -2.454 -24.727, -14.000 -0.113 -22.468, -14.000 -2.767 -25.493, -14.000 4.657 -22.765, -14.000 5.330 -23.270, -14.000 6.929 -26.162, -14.000 6.719 -25.348, -14.000 7.000 -27.000, -14.000 6.955 -23.936, -14.000 6.375 -24.580, -14.000 6.431 -24.230, -14.000 5.908 -23.881, -14.000 -5.932 -28.044, -11.500 -5.932 -28.044, -11.500 -5.727 -29.071, -14.000 -5.391 -30.061, -11.500 -5.391 -30.061, -14.000 -4.928 -31.000, -11.500 -4.928 -31.000, -14.000 -4.347 -31.870, -11.500 -4.347 -31.870, -11.500 -3.657 -32.657, -14.000 -3.657 -32.657, -14.000 -2.870 -33.347, -11.500 -2.870 -33.347, -11.500 -2.000 -33.928, -14.000 -2.000 -33.928, -11.500 -4.000 -26.067, -11.500 -5.000 -24.354, -11.500 2.303 -22.009, -11.500 1.476 -22.028, -11.500 3.122 -22.127, -11.500 3.910 -22.379, -11.500 -1.472 -23.402, -11.500 -2.018 -24.024, -11.500 -0.831 -22.879, -11.500 -2.454 -24.727, -11.500 -0.113 -22.468, -11.500 -2.767 -25.493, -11.500 0.663 -22.182, -11.500 4.657 -22.765, -11.500 5.330 -23.270, -11.500 7.000 -27.000, -11.500 6.955 -23.936, -11.500 6.375 -24.580, -11.500 6.431 -24.230, -11.500 5.908 -23.881, -11.500 -4.000 -27.000, -11.500 6.924 -27.868, -11.500 6.698 -28.710, -11.500 6.330 -29.500, -11.500 -2.924 -27.868, -11.500 -3.000 -27.000, -11.500 -2.698 -28.710, -11.500 4.500 -31.330, -11.500 -2.330 -29.500, -11.500 5.830 -30.214, -11.500 -1.830 -30.214, -11.500 5.214 -30.830, -11.500 -1.214 -30.830, -11.500 -0.500 -31.330, -14.000 6.924 -27.868, -14.000 6.698 -28.710, -14.000 6.330 -29.500, -14.000 -2.698 -28.710, -14.000 -2.330 -29.500, -14.000 -1.830 -30.214, -14.000 -1.214 -30.830, -14.000 -0.500 -31.330, -11.500 -2.896 -25.986, -14.000 -2.896 -25.986, -14.000 -2.974 -26.490, -11.500 -2.974 -26.490, -14.000 -3.384 -26.246, -11.500 -3.384 -26.246 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 2, 1, 3, -1, 4, 5, 6, -1, 5, 7, 6, -1, 6, 7, 8, -1, 7, 9, 8, -1, 10, 11, 12, -1, 11, 13, 12, -1, 14, 8, 15, -1, 14, 15, 16, -1, 2, 3, 17, -1, 17, 3, 18, -1, 14, 16, 19, -1, 19, 16, 20, -1, 21, 22, 23, -1, 22, 21, 24, -1, 24, 25, 22, -1, 26, 27, 28, -1, 26, 28, 29, -1, 30, 31, 32, -1, 33, 30, 32, -1, 34, 26, 3, -1, 34, 3, 1, -1, 35, 34, 1, -1, 35, 1, 36, -1, 37, 38, 39, -1, 37, 39, 40, -1, 41, 42, 43, -1, 42, 44, 43, -1, 43, 45, 46, -1, 46, 45, 47, -1, 46, 47, 48, -1, 48, 47, 49, -1, 48, 50, 51, -1, 51, 50, 52, -1, 51, 52, 53, -1, 53, 52, 54, -1, 53, 55, 56, -1, 56, 55, 57, -1, 21, 35, 20, -1, 35, 36, 20, -1, 20, 16, 21, -1, 16, 24, 21, -1, 16, 15, 24, -1, 15, 58, 24, -1, 15, 9, 58, -1, 9, 59, 58, -1, 9, 7, 59, -1, 7, 60, 59, -1, 60, 7, 5, -1, 61, 60, 5, -1, 61, 5, 62, -1, 63, 61, 62, -1, 62, 64, 63, -1, 64, 65, 63, -1, 64, 66, 65, -1, 66, 67, 68, -1, 68, 65, 66, -1, 67, 69, 68, -1, 70, 71, 69, -1, 67, 70, 69, -1, 70, 72, 71, -1, 73, 74, 75, -1, 73, 75, 76, -1, 75, 77, 76, -1, 77, 75, 78, -1, 75, 79, 78, -1, 80, 81, 82, -1, 80, 82, 83, -1, 83, 82, 84, -1, 84, 82, 85, -1, 85, 82, 86, -1, 79, 82, 81, -1, 87, 88, 89, -1, 88, 87, 90, -1, 91, 92, 93, -1, 92, 91, 94, -1, 92, 94, 95, -1, 92, 95, 37, -1, 92, 37, 40, -1, 96, 93, 92, -1, 97, 98, 99, -1, 97, 99, 100, -1, 97, 100, 101, -1, 102, 98, 97, -1, 103, 102, 97, -1, 104, 105, 106, -1, 104, 106, 107, -1, 108, 105, 104, -1, 109, 110, 108, -1, 110, 111, 108, -1, 108, 111, 105, -1, 109, 108, 104, -1, 112, 113, 114, -1, 115, 113, 112, -1, 116, 113, 115, -1, 117, 118, 119, -1, 120, 117, 119, -1, 121, 122, 52, -1, 122, 54, 52, -1, 52, 50, 121, -1, 123, 121, 50, -1, 50, 49, 123, -1, 124, 123, 49, -1, 49, 47, 124, -1, 125, 124, 47, -1, 47, 45, 125, -1, 45, 126, 125, -1, 45, 44, 126, -1, 44, 127, 126, -1, 44, 128, 127, -1, 128, 44, 42, -1, 129, 128, 42, -1, 42, 130, 129, -1, 131, 129, 130, -1, 130, 132, 131, -1, 132, 133, 131, -1, 132, 134, 133, -1, 135, 133, 134, -1, 134, 136, 135, -1, 136, 137, 135, -1, 136, 138, 137, -1, 138, 139, 137, -1, 138, 140, 139, -1, 140, 141, 139, -1, 142, 143, 144, -1, 142, 145, 143, -1, 142, 144, 145, -1, 144, 146, 145, -1, 107, 147, 148, -1, 148, 147, 149, -1, 149, 147, 150, -1, 150, 147, 151, -1, 152, 147, 107, -1, 147, 152, 151, -1, 153, 151, 154, -1, 154, 155, 153, -1, 156, 157, 158, -1, 156, 158, 159, -1, 111, 160, 161, -1, 160, 162, 161, -1, 163, 164, 165, -1, 165, 164, 166, -1, 166, 164, 167, -1, 167, 164, 113, -1, 168, 164, 163, -1, 168, 169, 141, -1, 168, 141, 170, -1, 171, 172, 173, -1, 171, 174, 172, -1, 171, 173, 174, -1, 175, 174, 173, -1, 175, 176, 174, -1, 175, 173, 176, -1, 177, 178, 179, -1, 180, 178, 177, -1, 180, 181, 178, -1, 179, 178, 182, -1, 179, 182, 183, -1, 179, 183, 184, -1, 184, 183, 185, -1, 184, 185, 186, -1, 155, 174, 176, -1, 155, 176, 187, -1, 188, 184, 186, -1, 188, 186, 189, -1, 190, 188, 189, -1, 190, 189, 191, -1, 153, 155, 187, -1, 153, 187, 192, -1, 193, 153, 192, -1, 193, 192, 194, -1, 195, 193, 196, -1, 195, 196, 170, -1, 197, 190, 198, -1, 197, 198, 199, -1, 200, 201, 202, -1, 202, 201, 203, -1, 203, 201, 204, -1, 204, 201, 205, -1, 206, 207, 208, -1, 207, 206, 209, -1, 209, 210, 207, -1, 211, 212, 201, -1, 213, 212, 211, -1, 214, 215, 216, -1, 217, 214, 216, -1, 192, 187, 218, -1, 187, 219, 218, -1, 220, 221, 222, -1, 222, 221, 223, -1, 220, 224, 221, -1, 221, 224, 225, -1, 226, 227, 228, -1, 229, 228, 227, -1, 230, 223, 231, -1, 230, 231, 232, -1, 230, 232, 233, -1, 234, 235, 236, -1, 234, 236, 237, -1, 237, 236, 230, -1, 238, 239, 240, -1, 238, 240, 241, -1, 238, 241, 225, -1, 240, 242, 241, -1, 242, 243, 241, -1, 244, 245, 246, -1, 244, 247, 245, -1, 244, 243, 247, -1, 244, 246, 248, -1, 249, 244, 248, -1, 250, 244, 249, -1, 250, 249, 251, -1, 252, 250, 251, -1, 252, 253, 250, -1, 254, 253, 252, -1, 255, 253, 254, -1, 255, 254, 256, -1, 257, 255, 256, -1, 258, 257, 256, -1, 258, 259, 257, -1, 258, 256, 259, -1, 260, 261, 257, -1, 260, 259, 261, -1, 260, 257, 259, -1, 261, 259, 262, -1, 263, 261, 262, -1, 263, 262, 264, -1, 265, 263, 264, -1, 266, 265, 264, -1, 267, 265, 266, -1, 267, 268, 265, -1, 269, 268, 267, -1, 270, 268, 269, -1, 271, 270, 269, -1, 272, 273, 274, -1, 272, 274, 275, -1, 275, 274, 276, -1, 277, 275, 276, -1, 278, 275, 277, -1, 279, 278, 277, -1, 279, 280, 281, -1, 279, 277, 280, -1, 279, 281, 282, -1, 283, 282, 281, -1, 284, 282, 283, -1, 285, 284, 283, -1, 285, 283, 286, -1, 285, 286, 287, -1, 287, 286, 288, -1, 287, 288, 289, -1, 289, 288, 270, -1, 289, 270, 271, -1, 273, 290, 274, -1, 291, 292, 293, -1, 294, 291, 293, -1, 295, 291, 294, -1, 293, 292, 296, -1, 292, 297, 296, -1, 298, 295, 294, -1, 298, 299, 295, -1, 300, 296, 297, -1, 301, 299, 298, -1, 301, 302, 299, -1, 303, 302, 301, -1, 304, 302, 303, -1, 305, 304, 303, -1, 306, 304, 305, -1, 307, 306, 305, -1, 307, 305, 308, -1, 309, 307, 308, -1, 309, 308, 310, -1, 311, 309, 310, -1, 311, 310, 312, -1, 313, 311, 312, -1, 314, 313, 312, -1, 315, 313, 314, -1, 316, 315, 314, -1, 316, 290, 315, -1, 316, 314, 290, -1, 273, 315, 290, -1, 249, 317, 251, -1, 249, 318, 317, -1, 249, 248, 318, -1, 248, 319, 318, -1, 246, 319, 248, -1, 246, 320, 319, -1, 246, 245, 321, -1, 320, 246, 321, -1, 322, 245, 247, -1, 322, 321, 245, -1, 323, 247, 243, -1, 323, 322, 247, -1, 323, 243, 324, -1, 325, 323, 324, -1, 243, 325, 324, -1, 242, 325, 243, -1, 326, 242, 327, -1, 326, 325, 242, -1, 326, 327, 325, -1, 327, 323, 325, -1, 250, 328, 244, -1, 244, 329, 243, -1, 329, 241, 243, -1, 328, 241, 329, -1, 328, 329, 244, -1, 250, 253, 330, -1, 330, 331, 250, -1, 332, 331, 333, -1, 334, 333, 331, -1, 330, 334, 331, -1, 332, 250, 331, -1, 288, 268, 270, -1, 257, 335, 255, -1, 335, 253, 255, -1, 261, 336, 257, -1, 263, 336, 261, -1, 265, 336, 263, -1, 268, 336, 265, -1, 288, 336, 268, -1, 337, 338, 335, -1, 337, 336, 338, -1, 337, 335, 336, -1, 288, 338, 336, -1, 336, 335, 257, -1, 280, 339, 281, -1, 280, 340, 339, -1, 340, 280, 341, -1, 281, 339, 342, -1, 283, 281, 343, -1, 281, 342, 343, -1, 283, 343, 344, -1, 288, 286, 345, -1, 286, 346, 345, -1, 347, 233, 345, -1, 348, 347, 345, -1, 349, 350, 347, -1, 349, 347, 351, -1, 352, 351, 347, -1, 348, 352, 347, -1, 347, 350, 230, -1, 347, 230, 233, -1, 345, 353, 348, -1, 345, 354, 353, -1, 345, 346, 354, -1, 283, 344, 355, -1, 356, 230, 350, -1, 357, 230, 356, -1, 237, 230, 357, -1, 286, 283, 358, -1, 283, 355, 358, -1, 288, 345, 338, -1, 286, 358, 346, -1, 274, 290, 276, -1, 293, 296, 236, -1, 296, 300, 236, -1, 235, 359, 236, -1, 360, 341, 359, -1, 360, 359, 361, -1, 361, 359, 362, -1, 362, 359, 235, -1, 314, 312, 359, -1, 312, 310, 359, -1, 359, 310, 308, -1, 359, 308, 305, -1, 359, 305, 303, -1, 276, 290, 277, -1, 303, 301, 359, -1, 301, 298, 359, -1, 359, 298, 294, -1, 359, 294, 293, -1, 341, 314, 359, -1, 359, 293, 236, -1, 280, 277, 341, -1, 341, 290, 314, -1, 290, 341, 277, -1, 297, 363, 300, -1, 363, 364, 300, -1, 364, 365, 300, -1, 300, 365, 366, -1, 365, 367, 366, -1, 365, 368, 367, -1, 368, 369, 367, -1, 366, 367, 370, -1, 369, 370, 367, -1, 254, 252, 218, -1, 254, 218, 219, -1, 256, 254, 219, -1, 219, 371, 256, -1, 371, 259, 256, -1, 371, 372, 259, -1, 262, 259, 372, -1, 262, 372, 373, -1, 373, 264, 262, -1, 373, 374, 264, -1, 266, 264, 374, -1, 266, 374, 375, -1, 267, 266, 375, -1, 375, 376, 267, -1, 376, 269, 267, -1, 377, 271, 269, -1, 376, 377, 269, -1, 377, 378, 271, -1, 379, 271, 378, -1, 379, 378, 380, -1, 229, 379, 380, -1, 380, 381, 229, -1, 381, 228, 229, -1, 381, 382, 228, -1, 383, 228, 382, -1, 383, 382, 384, -1, 385, 383, 384, -1, 385, 384, 386, -1, 387, 385, 386, -1, 387, 386, 388, -1, 389, 387, 388, -1, 388, 390, 389, -1, 391, 389, 390, -1, 391, 390, 392, -1, 392, 393, 391, -1, 393, 392, 394, -1, 394, 395, 393, -1, 395, 394, 396, -1, 397, 395, 396, -1, 397, 396, 398, -1, 398, 272, 397, -1, 398, 214, 272, -1, 273, 272, 214, -1, 214, 217, 273, -1, 217, 315, 273, -1, 217, 399, 315, -1, 313, 315, 399, -1, 399, 400, 313, -1, 400, 311, 313, -1, 311, 400, 401, -1, 401, 309, 311, -1, 309, 401, 402, -1, 402, 307, 309, -1, 307, 402, 403, -1, 403, 306, 307, -1, 403, 404, 306, -1, 404, 304, 306, -1, 304, 404, 405, -1, 405, 302, 304, -1, 302, 405, 406, -1, 406, 299, 302, -1, 299, 406, 407, -1, 295, 299, 407, -1, 407, 408, 295, -1, 291, 295, 408, -1, 291, 408, 409, -1, 292, 291, 409, -1, 292, 409, 410, -1, 410, 297, 292, -1, 297, 410, 411, -1, 411, 363, 297, -1, 363, 411, 412, -1, 412, 364, 363, -1, 412, 413, 364, -1, 413, 365, 364, -1, 365, 413, 414, -1, 227, 415, 229, -1, 229, 415, 379, -1, 272, 275, 397, -1, 379, 415, 289, -1, 397, 416, 395, -1, 379, 289, 271, -1, 416, 397, 275, -1, 393, 395, 417, -1, 395, 416, 417, -1, 418, 391, 393, -1, 418, 419, 391, -1, 418, 393, 419, -1, 393, 417, 419, -1, 420, 389, 391, -1, 420, 421, 389, -1, 420, 391, 421, -1, 391, 419, 421, -1, 422, 387, 389, -1, 422, 423, 387, -1, 422, 389, 423, -1, 389, 421, 423, -1, 424, 385, 387, -1, 424, 425, 385, -1, 424, 387, 425, -1, 426, 387, 423, -1, 426, 425, 387, -1, 426, 423, 425, -1, 385, 425, 427, -1, 428, 385, 427, -1, 428, 383, 385, -1, 428, 427, 383, -1, 427, 226, 383, -1, 429, 383, 226, -1, 429, 228, 383, -1, 429, 226, 228, -1, 419, 278, 421, -1, 417, 278, 419, -1, 416, 278, 417, -1, 275, 278, 416, -1, 421, 278, 279, -1, 421, 279, 282, -1, 427, 284, 226, -1, 284, 227, 226, -1, 227, 285, 415, -1, 227, 284, 285, -1, 415, 285, 287, -1, 415, 287, 289, -1, 427, 425, 430, -1, 431, 425, 423, -1, 431, 430, 425, -1, 431, 423, 430, -1, 423, 421, 430, -1, 282, 284, 430, -1, 432, 421, 282, -1, 432, 430, 421, -1, 432, 282, 430, -1, 430, 284, 427, -1, 252, 251, 218, -1, 251, 317, 218, -1, 318, 196, 317, -1, 317, 196, 194, -1, 196, 318, 319, -1, 170, 196, 319, -1, 319, 168, 170, -1, 320, 168, 319, -1, 168, 320, 321, -1, 168, 321, 164, -1, 321, 322, 164, -1, 433, 164, 322, -1, 327, 242, 240, -1, 327, 240, 434, -1, 433, 322, 323, -1, 323, 435, 433, -1, 323, 327, 436, -1, 327, 437, 436, -1, 323, 436, 438, -1, 436, 437, 438, -1, 437, 439, 438, -1, 240, 239, 440, -1, 240, 440, 434, -1, 440, 239, 238, -1, 368, 440, 238, -1, 365, 441, 368, -1, 414, 441, 365, -1, 414, 440, 441, -1, 441, 440, 368, -1, 368, 238, 225, -1, 369, 368, 224, -1, 369, 224, 442, -1, 442, 224, 443, -1, 443, 224, 444, -1, 444, 224, 220, -1, 224, 368, 225, -1, 445, 225, 241, -1, 446, 445, 241, -1, 446, 328, 445, -1, 446, 241, 328, -1, 250, 445, 328, -1, 332, 447, 250, -1, 221, 445, 447, -1, 233, 221, 447, -1, 233, 447, 448, -1, 448, 447, 449, -1, 332, 449, 447, -1, 447, 445, 250, -1, 335, 338, 450, -1, 338, 332, 450, -1, 335, 450, 253, -1, 332, 333, 450, -1, 333, 334, 450, -1, 450, 334, 330, -1, 451, 330, 253, -1, 451, 450, 330, -1, 451, 253, 450, -1, 452, 356, 350, -1, 356, 452, 453, -1, 350, 349, 454, -1, 454, 452, 350, -1, 349, 455, 454, -1, 349, 351, 455, -1, 456, 455, 351, -1, 456, 351, 352, -1, 352, 162, 456, -1, 162, 352, 348, -1, 348, 457, 162, -1, 457, 348, 353, -1, 458, 457, 353, -1, 458, 353, 354, -1, 459, 458, 354, -1, 354, 346, 459, -1, 346, 460, 459, -1, 460, 346, 358, -1, 358, 461, 460, -1, 358, 355, 461, -1, 355, 462, 461, -1, 355, 344, 462, -1, 344, 463, 462, -1, 344, 343, 463, -1, 464, 463, 343, -1, 464, 343, 342, -1, 342, 211, 464, -1, 342, 339, 211, -1, 213, 211, 339, -1, 213, 339, 340, -1, 465, 213, 340, -1, 340, 341, 465, -1, 341, 466, 465, -1, 466, 341, 360, -1, 467, 466, 360, -1, 467, 360, 361, -1, 361, 468, 467, -1, 361, 362, 468, -1, 362, 469, 468, -1, 469, 362, 235, -1, 470, 469, 235, -1, 470, 235, 234, -1, 471, 470, 234, -1, 471, 234, 237, -1, 472, 471, 237, -1, 472, 237, 357, -1, 357, 473, 472, -1, 474, 473, 357, -1, 357, 356, 474, -1, 356, 453, 474, -1, 230, 236, 475, -1, 475, 236, 369, -1, 475, 369, 442, -1, 475, 442, 443, -1, 475, 443, 444, -1, 230, 475, 223, -1, 475, 444, 220, -1, 475, 220, 222, -1, 475, 222, 223, -1, 223, 221, 476, -1, 223, 476, 231, -1, 232, 231, 476, -1, 232, 476, 233, -1, 233, 476, 221, -1, 338, 477, 332, -1, 338, 345, 477, -1, 477, 233, 448, -1, 477, 448, 449, -1, 477, 449, 332, -1, 345, 233, 477, -1, 300, 478, 236, -1, 300, 366, 478, -1, 479, 366, 370, -1, 479, 478, 366, -1, 479, 370, 478, -1, 369, 478, 370, -1, 480, 369, 236, -1, 480, 478, 369, -1, 480, 236, 478, -1, 371, 219, 176, -1, 187, 176, 219, -1, 173, 372, 371, -1, 371, 176, 173, -1, 199, 373, 372, -1, 173, 199, 372, -1, 373, 199, 198, -1, 198, 374, 373, -1, 374, 198, 191, -1, 375, 374, 191, -1, 191, 189, 375, -1, 189, 376, 375, -1, 376, 189, 186, -1, 186, 377, 376, -1, 377, 186, 185, -1, 481, 381, 380, -1, 482, 381, 481, -1, 481, 380, 483, -1, 484, 380, 378, -1, 484, 483, 380, -1, 484, 378, 483, -1, 214, 398, 485, -1, 483, 378, 486, -1, 486, 378, 487, -1, 487, 378, 488, -1, 485, 398, 489, -1, 488, 378, 377, -1, 490, 386, 384, -1, 490, 388, 386, -1, 390, 388, 490, -1, 489, 398, 491, -1, 492, 390, 490, -1, 492, 493, 390, -1, 492, 490, 493, -1, 494, 493, 490, -1, 494, 495, 493, -1, 494, 490, 495, -1, 496, 495, 490, -1, 496, 384, 495, -1, 496, 490, 384, -1, 497, 396, 394, -1, 498, 396, 497, -1, 498, 398, 396, -1, 498, 497, 398, -1, 491, 398, 497, -1, 499, 392, 390, -1, 394, 392, 499, -1, 497, 394, 499, -1, 499, 390, 493, -1, 482, 382, 381, -1, 482, 384, 382, -1, 495, 384, 482, -1, 500, 408, 407, -1, 501, 500, 407, -1, 502, 409, 408, -1, 408, 500, 502, -1, 399, 217, 503, -1, 217, 216, 503, -1, 504, 410, 409, -1, 502, 504, 409, -1, 400, 399, 505, -1, 411, 410, 506, -1, 503, 505, 399, -1, 410, 504, 506, -1, 401, 400, 507, -1, 400, 505, 507, -1, 402, 401, 508, -1, 507, 508, 401, -1, 509, 403, 402, -1, 508, 509, 402, -1, 404, 403, 510, -1, 403, 509, 510, -1, 511, 405, 404, -1, 404, 510, 511, -1, 512, 406, 405, -1, 512, 501, 406, -1, 511, 512, 405, -1, 501, 407, 406, -1, 465, 212, 213, -1, 466, 210, 465, -1, 513, 514, 466, -1, 514, 210, 466, -1, 514, 207, 210, -1, 411, 515, 412, -1, 412, 515, 413, -1, 513, 466, 467, -1, 516, 513, 467, -1, 517, 516, 467, -1, 411, 518, 515, -1, 517, 467, 468, -1, 519, 517, 468, -1, 520, 519, 468, -1, 413, 515, 414, -1, 520, 468, 469, -1, 521, 520, 469, -1, 522, 521, 469, -1, 146, 471, 472, -1, 523, 471, 146, -1, 523, 146, 524, -1, 524, 146, 518, -1, 145, 146, 472, -1, 525, 201, 212, -1, 522, 469, 470, -1, 526, 522, 470, -1, 525, 205, 201, -1, 523, 526, 470, -1, 523, 470, 471, -1, 465, 210, 212, -1, 146, 515, 518, -1, 202, 527, 200, -1, 527, 528, 200, -1, 200, 528, 529, -1, 529, 530, 200, -1, 152, 200, 530, -1, 152, 530, 531, -1, 152, 531, 532, -1, 152, 532, 533, -1, 218, 317, 192, -1, 317, 194, 192, -1, 433, 435, 195, -1, 195, 170, 433, -1, 193, 194, 196, -1, 190, 191, 198, -1, 172, 197, 199, -1, 172, 199, 173, -1, 168, 534, 169, -1, 535, 534, 168, -1, 536, 535, 168, -1, 120, 536, 168, -1, 117, 120, 168, -1, 117, 168, 537, -1, 537, 168, 538, -1, 538, 168, 539, -1, 539, 168, 540, -1, 168, 541, 540, -1, 168, 542, 541, -1, 168, 543, 542, -1, 543, 168, 163, -1, 544, 113, 164, -1, 544, 164, 433, -1, 327, 434, 545, -1, 434, 546, 545, -1, 437, 327, 545, -1, 438, 195, 323, -1, 195, 435, 323, -1, 437, 547, 439, -1, 548, 193, 195, -1, 548, 549, 193, -1, 548, 195, 549, -1, 550, 551, 193, -1, 550, 549, 551, -1, 550, 193, 549, -1, 549, 547, 551, -1, 552, 195, 438, -1, 552, 549, 195, -1, 552, 438, 549, -1, 549, 439, 547, -1, 549, 438, 439, -1, 440, 553, 434, -1, 554, 553, 414, -1, 553, 554, 555, -1, 553, 555, 143, -1, 434, 553, 143, -1, 414, 553, 440, -1, 221, 225, 556, -1, 221, 556, 445, -1, 556, 225, 445, -1, 459, 460, 161, -1, 460, 200, 161, -1, 145, 453, 452, -1, 145, 472, 473, -1, 145, 473, 474, -1, 211, 201, 464, -1, 145, 474, 453, -1, 464, 201, 463, -1, 161, 200, 152, -1, 463, 201, 462, -1, 462, 201, 461, -1, 452, 454, 160, -1, 461, 200, 460, -1, 454, 455, 160, -1, 455, 456, 160, -1, 456, 162, 160, -1, 461, 201, 200, -1, 145, 452, 160, -1, 145, 160, 111, -1, 557, 145, 558, -1, 558, 145, 559, -1, 145, 111, 559, -1, 162, 457, 161, -1, 457, 458, 161, -1, 458, 459, 161, -1, 487, 560, 486, -1, 486, 560, 183, -1, 560, 185, 183, -1, 488, 560, 487, -1, 377, 185, 560, -1, 377, 560, 488, -1, 491, 561, 489, -1, 489, 561, 485, -1, 214, 159, 215, -1, 485, 159, 214, -1, 159, 158, 215, -1, 562, 485, 561, -1, 562, 159, 485, -1, 562, 561, 159, -1, 156, 159, 563, -1, 561, 563, 159, -1, 483, 486, 564, -1, 483, 564, 178, -1, 564, 182, 178, -1, 486, 183, 564, -1, 564, 183, 182, -1, 483, 178, 481, -1, 481, 178, 181, -1, 481, 565, 482, -1, 482, 565, 566, -1, 565, 96, 566, -1, 565, 567, 96, -1, 181, 567, 565, -1, 568, 481, 181, -1, 568, 565, 481, -1, 568, 181, 565, -1, 495, 569, 493, -1, 570, 571, 493, -1, 570, 569, 571, -1, 570, 493, 569, -1, 569, 566, 571, -1, 495, 482, 569, -1, 569, 482, 566, -1, 493, 572, 499, -1, 493, 571, 572, -1, 499, 572, 102, -1, 573, 497, 499, -1, 573, 574, 497, -1, 573, 499, 574, -1, 561, 574, 156, -1, 574, 561, 497, -1, 156, 574, 103, -1, 574, 102, 103, -1, 575, 499, 102, -1, 575, 574, 499, -1, 575, 102, 574, -1, 491, 497, 561, -1, 576, 215, 158, -1, 576, 158, 577, -1, 509, 508, 578, -1, 579, 509, 578, -1, 578, 508, 580, -1, 508, 507, 580, -1, 580, 507, 581, -1, 507, 505, 581, -1, 502, 582, 504, -1, 581, 505, 583, -1, 502, 500, 584, -1, 502, 584, 582, -1, 505, 503, 583, -1, 504, 585, 506, -1, 586, 503, 587, -1, 586, 583, 503, -1, 586, 587, 583, -1, 504, 582, 585, -1, 503, 216, 587, -1, 587, 216, 576, -1, 500, 501, 588, -1, 500, 588, 584, -1, 506, 585, 589, -1, 501, 512, 590, -1, 501, 590, 588, -1, 216, 215, 576, -1, 512, 511, 591, -1, 512, 591, 590, -1, 511, 510, 579, -1, 511, 579, 591, -1, 510, 509, 579, -1, 592, 576, 577, -1, 592, 577, 593, -1, 518, 411, 589, -1, 411, 506, 589, -1, 592, 206, 208, -1, 592, 208, 576, -1, 594, 209, 206, -1, 210, 209, 594, -1, 592, 594, 206, -1, 593, 594, 592, -1, 595, 594, 596, -1, 594, 597, 596, -1, 597, 594, 593, -1, 525, 212, 598, -1, 599, 525, 598, -1, 212, 600, 598, -1, 600, 212, 595, -1, 601, 210, 594, -1, 602, 210, 601, -1, 602, 212, 210, -1, 602, 601, 212, -1, 601, 594, 595, -1, 603, 212, 601, -1, 603, 595, 212, -1, 603, 601, 595, -1, 604, 599, 605, -1, 604, 525, 599, -1, 604, 605, 525, -1, 605, 205, 525, -1, 606, 605, 599, -1, 204, 205, 605, -1, 204, 605, 606, -1, 177, 202, 203, -1, 177, 203, 180, -1, 204, 606, 607, -1, 203, 204, 607, -1, 79, 607, 606, -1, 607, 79, 608, -1, 180, 607, 608, -1, 609, 203, 607, -1, 609, 180, 203, -1, 609, 607, 180, -1, 610, 177, 611, -1, 610, 202, 177, -1, 610, 611, 202, -1, 612, 202, 611, -1, 612, 527, 202, -1, 612, 611, 527, -1, 179, 611, 177, -1, 611, 528, 527, -1, 611, 179, 184, -1, 613, 528, 611, -1, 613, 184, 528, -1, 613, 611, 184, -1, 154, 614, 155, -1, 614, 174, 155, -1, 174, 614, 533, -1, 533, 532, 174, -1, 172, 174, 532, -1, 172, 532, 531, -1, 197, 172, 531, -1, 531, 530, 197, -1, 530, 190, 197, -1, 190, 530, 529, -1, 188, 190, 529, -1, 188, 529, 528, -1, 528, 184, 188, -1, 152, 533, 614, -1, 154, 152, 614, -1, 151, 152, 154, -1, 414, 144, 554, -1, 554, 144, 555, -1, 555, 144, 143, -1, 515, 146, 144, -1, 414, 515, 144, -1, 582, 584, 523, -1, 582, 523, 524, -1, 208, 207, 576, -1, 524, 585, 582, -1, 524, 518, 585, -1, 587, 576, 207, -1, 589, 585, 518, -1, 207, 514, 587, -1, 514, 583, 587, -1, 583, 514, 513, -1, 513, 581, 583, -1, 581, 513, 516, -1, 516, 580, 581, -1, 516, 517, 580, -1, 517, 578, 580, -1, 578, 517, 519, -1, 519, 579, 578, -1, 579, 519, 520, -1, 520, 591, 579, -1, 520, 521, 591, -1, 521, 590, 591, -1, 590, 521, 522, -1, 522, 588, 590, -1, 522, 526, 588, -1, 584, 588, 526, -1, 584, 526, 523, -1, 433, 615, 544, -1, 616, 617, 433, -1, 617, 618, 433, -1, 433, 618, 615, -1, 170, 616, 433, -1, 141, 619, 170, -1, 620, 170, 619, -1, 621, 170, 620, -1, 622, 170, 621, -1, 623, 170, 622, -1, 170, 623, 624, -1, 170, 624, 625, -1, 170, 625, 626, -1, 170, 626, 627, -1, 170, 627, 628, -1, 170, 628, 629, -1, 170, 629, 630, -1, 630, 616, 170, -1, 150, 151, 153, -1, 104, 193, 551, -1, 631, 153, 193, -1, 153, 631, 150, -1, 150, 631, 104, -1, 631, 193, 104, -1, 181, 180, 632, -1, 181, 632, 72, -1, 141, 169, 139, -1, 539, 540, 118, -1, 118, 540, 541, -1, 118, 541, 542, -1, 118, 542, 543, -1, 118, 543, 163, -1, 118, 163, 633, -1, 116, 167, 113, -1, 120, 119, 634, -1, 635, 165, 166, -1, 536, 120, 634, -1, 635, 166, 167, -1, 635, 167, 116, -1, 535, 536, 634, -1, 535, 634, 636, -1, 534, 535, 636, -1, 534, 636, 637, -1, 169, 534, 637, -1, 638, 119, 118, -1, 633, 163, 165, -1, 638, 118, 639, -1, 639, 118, 640, -1, 118, 641, 640, -1, 633, 165, 635, -1, 118, 633, 641, -1, 117, 537, 118, -1, 537, 538, 118, -1, 538, 539, 118, -1, 113, 544, 642, -1, 642, 643, 113, -1, 644, 643, 642, -1, 644, 642, 645, -1, 644, 645, 646, -1, 647, 644, 646, -1, 646, 648, 647, -1, 648, 646, 649, -1, 648, 649, 650, -1, 650, 651, 648, -1, 650, 652, 651, -1, 650, 653, 652, -1, 652, 653, 654, -1, 655, 652, 654, -1, 655, 654, 656, -1, 656, 657, 655, -1, 657, 656, 658, -1, 658, 659, 657, -1, 659, 658, 660, -1, 661, 659, 660, -1, 661, 660, 662, -1, 663, 661, 662, -1, 662, 664, 663, -1, 665, 663, 664, -1, 664, 666, 665, -1, 666, 667, 665, -1, 666, 668, 667, -1, 668, 669, 667, -1, 669, 668, 670, -1, 670, 671, 669, -1, 671, 670, 672, -1, 672, 673, 671, -1, 673, 672, 674, -1, 675, 673, 674, -1, 675, 674, 676, -1, 677, 675, 676, -1, 676, 678, 677, -1, 676, 679, 678, -1, 678, 679, 680, -1, 680, 681, 678, -1, 681, 680, 682, -1, 683, 681, 682, -1, 682, 684, 683, -1, 685, 683, 684, -1, 685, 684, 686, -1, 686, 687, 685, -1, 686, 57, 687, -1, 688, 687, 57, -1, 689, 688, 57, -1, 689, 57, 55, -1, 122, 689, 55, -1, 122, 55, 54, -1, 434, 690, 546, -1, 690, 557, 110, -1, 546, 690, 110, -1, 143, 690, 434, -1, 690, 143, 691, -1, 690, 691, 692, -1, 690, 692, 693, -1, 690, 693, 694, -1, 690, 694, 695, -1, 690, 695, 557, -1, 545, 546, 437, -1, 437, 546, 109, -1, 109, 104, 551, -1, 437, 109, 551, -1, 437, 551, 547, -1, 107, 696, 152, -1, 111, 696, 105, -1, 105, 696, 106, -1, 106, 696, 107, -1, 161, 696, 111, -1, 696, 161, 152, -1, 110, 697, 111, -1, 557, 697, 110, -1, 697, 557, 558, -1, 697, 558, 559, -1, 697, 559, 111, -1, 143, 145, 691, -1, 691, 145, 692, -1, 692, 145, 693, -1, 693, 145, 694, -1, 694, 145, 695, -1, 695, 145, 557, -1, 97, 158, 157, -1, 97, 157, 698, -1, 103, 97, 698, -1, 158, 97, 101, -1, 699, 158, 700, -1, 158, 101, 700, -1, 103, 698, 156, -1, 156, 698, 157, -1, 561, 156, 563, -1, 102, 701, 98, -1, 102, 702, 703, -1, 102, 703, 701, -1, 572, 702, 102, -1, 566, 92, 571, -1, 96, 92, 566, -1, 571, 92, 40, -1, 96, 704, 93, -1, 96, 88, 704, -1, 88, 90, 705, -1, 705, 704, 88, -1, 571, 40, 702, -1, 571, 702, 572, -1, 88, 181, 89, -1, 706, 181, 707, -1, 708, 181, 706, -1, 89, 181, 708, -1, 181, 88, 567, -1, 96, 567, 88, -1, 181, 72, 707, -1, 158, 709, 577, -1, 699, 709, 158, -1, 709, 593, 577, -1, 710, 709, 699, -1, 710, 593, 709, -1, 710, 711, 593, -1, 593, 711, 712, -1, 713, 597, 593, -1, 713, 596, 597, -1, 713, 595, 596, -1, 713, 600, 595, -1, 714, 715, 713, -1, 716, 714, 713, -1, 712, 716, 713, -1, 715, 600, 713, -1, 713, 593, 712, -1, 600, 715, 717, -1, 718, 600, 719, -1, 719, 600, 717, -1, 598, 600, 718, -1, 82, 606, 599, -1, 82, 79, 606, -1, 86, 82, 599, -1, 78, 79, 81, -1, 86, 599, 718, -1, 718, 599, 598, -1, 75, 74, 180, -1, 30, 33, 180, -1, 720, 30, 180, -1, 74, 720, 180, -1, 75, 180, 608, -1, 608, 79, 75, -1, 180, 33, 632, -1, 721, 107, 148, -1, 721, 148, 149, -1, 721, 149, 150, -1, 104, 721, 150, -1, 107, 721, 104, -1, 624, 623, 722, -1, 625, 624, 722, -1, 625, 722, 626, -1, 626, 722, 627, -1, 627, 722, 628, -1, 628, 722, 629, -1, 629, 722, 630, -1, 630, 722, 616, -1, 616, 722, 723, -1, 722, 724, 725, -1, 722, 725, 726, -1, 722, 726, 727, -1, 722, 727, 723, -1, 623, 728, 722, -1, 615, 729, 544, -1, 544, 729, 730, -1, 544, 731, 732, -1, 544, 730, 731, -1, 617, 733, 618, -1, 618, 733, 615, -1, 623, 622, 728, -1, 622, 734, 728, -1, 615, 733, 729, -1, 622, 621, 734, -1, 621, 620, 734, -1, 620, 735, 734, -1, 616, 723, 617, -1, 620, 619, 735, -1, 619, 736, 735, -1, 617, 723, 733, -1, 619, 141, 736, -1, 724, 722, 728, -1, 632, 71, 72, -1, 114, 113, 643, -1, 114, 643, 644, -1, 114, 644, 647, -1, 114, 647, 648, -1, 114, 648, 651, -1, 737, 651, 652, -1, 737, 652, 655, -1, 737, 114, 651, -1, 738, 655, 657, -1, 738, 737, 655, -1, 739, 657, 659, -1, 739, 659, 661, -1, 739, 738, 657, -1, 740, 661, 663, -1, 740, 739, 661, -1, 741, 663, 665, -1, 741, 665, 667, -1, 741, 740, 663, -1, 742, 741, 667, -1, 742, 667, 669, -1, 743, 742, 669, -1, 743, 669, 671, -1, 743, 671, 673, -1, 744, 743, 673, -1, 744, 673, 675, -1, 677, 744, 675, -1, 745, 744, 677, -1, 678, 745, 677, -1, 746, 745, 678, -1, 681, 746, 678, -1, 683, 746, 681, -1, 683, 747, 746, -1, 685, 747, 683, -1, 685, 748, 747, -1, 687, 748, 685, -1, 688, 748, 687, -1, 688, 749, 748, -1, 689, 749, 688, -1, 689, 750, 749, -1, 122, 750, 689, -1, 139, 169, 637, -1, 139, 637, 751, -1, 121, 750, 122, -1, 121, 752, 750, -1, 137, 139, 751, -1, 137, 751, 753, -1, 123, 752, 121, -1, 123, 754, 752, -1, 135, 137, 753, -1, 124, 754, 123, -1, 133, 135, 753, -1, 133, 753, 755, -1, 125, 754, 124, -1, 125, 756, 754, -1, 131, 133, 755, -1, 131, 755, 757, -1, 126, 756, 125, -1, 126, 758, 756, -1, 129, 131, 757, -1, 127, 758, 126, -1, 128, 758, 127, -1, 128, 757, 758, -1, 128, 129, 757, -1, 544, 732, 642, -1, 642, 732, 645, -1, 645, 732, 646, -1, 646, 732, 649, -1, 649, 732, 650, -1, 650, 759, 653, -1, 759, 654, 653, -1, 732, 759, 650, -1, 760, 656, 654, -1, 759, 760, 654, -1, 761, 658, 656, -1, 761, 660, 658, -1, 760, 761, 656, -1, 762, 662, 660, -1, 761, 762, 660, -1, 763, 664, 662, -1, 763, 666, 664, -1, 762, 763, 662, -1, 763, 764, 666, -1, 764, 668, 666, -1, 764, 765, 668, -1, 765, 670, 668, -1, 765, 672, 670, -1, 765, 766, 672, -1, 766, 674, 672, -1, 766, 676, 674, -1, 766, 767, 676, -1, 767, 679, 676, -1, 768, 679, 767, -1, 768, 680, 679, -1, 768, 682, 680, -1, 769, 682, 768, -1, 769, 684, 682, -1, 770, 684, 769, -1, 770, 686, 684, -1, 770, 57, 686, -1, 56, 57, 770, -1, 53, 54, 55, -1, 141, 140, 736, -1, 140, 771, 736, -1, 140, 138, 771, -1, 138, 772, 771, -1, 138, 136, 772, -1, 48, 49, 50, -1, 136, 134, 772, -1, 134, 773, 772, -1, 134, 132, 773, -1, 132, 41, 773, -1, 132, 130, 41, -1, 43, 44, 45, -1, 130, 42, 41, -1, 53, 750, 752, -1, 51, 53, 752, -1, 48, 752, 754, -1, 48, 51, 752, -1, 46, 754, 756, -1, 46, 48, 754, -1, 43, 756, 758, -1, 756, 43, 46, -1, 758, 757, 41, -1, 758, 41, 43, -1, 757, 755, 773, -1, 757, 773, 41, -1, 755, 753, 773, -1, 753, 772, 773, -1, 753, 771, 772, -1, 753, 751, 771, -1, 751, 736, 771, -1, 751, 637, 736, -1, 637, 636, 736, -1, 735, 736, 636, -1, 636, 734, 735, -1, 636, 634, 734, -1, 734, 634, 119, -1, 119, 728, 734, -1, 119, 638, 728, -1, 724, 728, 638, -1, 724, 638, 639, -1, 725, 724, 639, -1, 725, 639, 640, -1, 726, 725, 640, -1, 640, 641, 726, -1, 727, 726, 641, -1, 641, 633, 727, -1, 723, 727, 633, -1, 633, 635, 723, -1, 733, 723, 635, -1, 635, 116, 733, -1, 729, 733, 116, -1, 729, 116, 115, -1, 115, 730, 729, -1, 115, 731, 730, -1, 731, 115, 112, -1, 112, 114, 731, -1, 114, 732, 731, -1, 732, 114, 737, -1, 759, 732, 737, -1, 760, 737, 738, -1, 760, 759, 737, -1, 761, 738, 739, -1, 738, 761, 760, -1, 739, 740, 762, -1, 762, 761, 739, -1, 763, 740, 741, -1, 763, 762, 740, -1, 741, 742, 764, -1, 764, 763, 741, -1, 742, 743, 765, -1, 742, 765, 764, -1, 743, 744, 766, -1, 766, 765, 743, -1, 744, 745, 767, -1, 744, 767, 766, -1, 768, 767, 745, -1, 745, 746, 768, -1, 769, 768, 746, -1, 769, 746, 747, -1, 747, 770, 769, -1, 770, 747, 748, -1, 56, 770, 748, -1, 56, 748, 749, -1, 749, 53, 56, -1, 53, 749, 750, -1, 774, 110, 109, -1, 546, 774, 109, -1, 110, 774, 546, -1, 98, 775, 99, -1, 101, 775, 700, -1, 700, 775, 699, -1, 99, 776, 100, -1, 100, 776, 101, -1, 776, 775, 101, -1, 99, 775, 776, -1, 91, 777, 94, -1, 94, 777, 95, -1, 95, 777, 38, -1, 91, 93, 778, -1, 91, 778, 777, -1, 704, 779, 93, -1, 93, 779, 778, -1, 705, 779, 704, -1, 701, 780, 98, -1, 90, 779, 705, -1, 780, 781, 98, -1, 702, 782, 703, -1, 703, 782, 701, -1, 782, 780, 701, -1, 98, 783, 775, -1, 98, 781, 783, -1, 40, 784, 702, -1, 702, 784, 782, -1, 783, 785, 775, -1, 40, 39, 784, -1, 95, 38, 37, -1, 90, 786, 779, -1, 90, 787, 786, -1, 87, 787, 90, -1, 706, 707, 788, -1, 706, 788, 789, -1, 707, 790, 788, -1, 707, 72, 790, -1, 708, 706, 791, -1, 89, 708, 791, -1, 87, 89, 791, -1, 87, 791, 787, -1, 791, 789, 792, -1, 793, 794, 791, -1, 793, 792, 794, -1, 793, 791, 792, -1, 791, 794, 787, -1, 706, 789, 791, -1, 699, 795, 710, -1, 795, 796, 710, -1, 795, 13, 796, -1, 797, 796, 13, -1, 797, 13, 798, -1, 798, 799, 797, -1, 798, 800, 799, -1, 801, 799, 800, -1, 801, 800, 802, -1, 803, 801, 802, -1, 804, 803, 802, -1, 804, 802, 805, -1, 804, 805, 806, -1, 807, 804, 806, -1, 806, 808, 807, -1, 806, 809, 808, -1, 808, 809, 18, -1, 29, 808, 18, -1, 18, 26, 29, -1, 26, 18, 3, -1, 715, 714, 810, -1, 810, 712, 711, -1, 810, 711, 710, -1, 714, 716, 811, -1, 811, 716, 712, -1, 810, 811, 712, -1, 810, 714, 811, -1, 812, 80, 83, -1, 812, 83, 84, -1, 812, 84, 813, -1, 81, 80, 814, -1, 814, 80, 812, -1, 815, 78, 81, -1, 815, 81, 814, -1, 77, 78, 815, -1, 717, 715, 816, -1, 76, 77, 815, -1, 816, 715, 817, -1, 818, 718, 719, -1, 818, 719, 717, -1, 818, 717, 816, -1, 715, 810, 819, -1, 715, 819, 817, -1, 820, 86, 718, -1, 820, 718, 818, -1, 819, 810, 821, -1, 822, 85, 86, -1, 813, 85, 822, -1, 822, 86, 820, -1, 813, 84, 85, -1, 76, 815, 823, -1, 824, 76, 823, -1, 73, 76, 824, -1, 33, 32, 825, -1, 632, 33, 825, -1, 30, 720, 826, -1, 720, 74, 826, -1, 74, 73, 826, -1, 826, 73, 824, -1, 31, 826, 827, -1, 828, 829, 827, -1, 828, 826, 829, -1, 828, 827, 826, -1, 826, 824, 829, -1, 30, 826, 31, -1, 830, 710, 796, -1, 69, 71, 831, -1, 69, 831, 832, -1, 830, 810, 710, -1, 797, 830, 796, -1, 68, 69, 832, -1, 68, 832, 833, -1, 834, 830, 797, -1, 835, 830, 834, -1, 836, 797, 799, -1, 834, 797, 836, -1, 58, 25, 24, -1, 837, 25, 58, -1, 65, 68, 833, -1, 838, 799, 801, -1, 59, 837, 58, -1, 836, 799, 838, -1, 838, 801, 803, -1, 63, 65, 833, -1, 63, 833, 839, -1, 838, 803, 840, -1, 60, 837, 59, -1, 60, 841, 837, -1, 34, 27, 26, -1, 34, 23, 27, -1, 840, 803, 804, -1, 61, 63, 839, -1, 61, 839, 841, -1, 61, 841, 60, -1, 842, 840, 804, -1, 807, 842, 804, -1, 843, 842, 807, -1, 808, 843, 807, -1, 35, 23, 34, -1, 29, 28, 843, -1, 71, 825, 831, -1, 71, 632, 825, -1, 29, 843, 808, -1, 21, 23, 35, -1, 70, 67, 844, -1, 699, 12, 795, -1, 67, 66, 845, -1, 699, 775, 12, -1, 66, 846, 845, -1, 795, 12, 13, -1, 66, 64, 846, -1, 8, 9, 15, -1, 13, 847, 798, -1, 13, 11, 847, -1, 64, 62, 846, -1, 846, 62, 4, -1, 798, 848, 800, -1, 798, 847, 848, -1, 62, 5, 4, -1, 800, 848, 802, -1, 848, 849, 802, -1, 0, 36, 1, -1, 802, 849, 805, -1, 849, 850, 805, -1, 850, 806, 805, -1, 851, 806, 850, -1, 851, 809, 806, -1, 70, 844, 790, -1, 72, 70, 790, -1, 19, 20, 0, -1, 17, 18, 851, -1, 0, 20, 36, -1, 851, 18, 809, -1, 67, 845, 844, -1, 0, 23, 22, -1, 22, 19, 0, -1, 14, 22, 25, -1, 14, 19, 22, -1, 8, 25, 837, -1, 25, 8, 14, -1, 837, 841, 6, -1, 837, 6, 8, -1, 4, 841, 839, -1, 4, 6, 841, -1, 846, 4, 839, -1, 839, 833, 846, -1, 846, 833, 832, -1, 832, 845, 846, -1, 844, 845, 832, -1, 832, 831, 844, -1, 790, 844, 831, -1, 831, 825, 790, -1, 825, 788, 790, -1, 825, 32, 788, -1, 789, 788, 32, -1, 32, 31, 789, -1, 31, 827, 792, -1, 792, 789, 31, -1, 794, 827, 829, -1, 794, 792, 827, -1, 787, 829, 824, -1, 829, 787, 794, -1, 786, 824, 823, -1, 786, 787, 824, -1, 779, 823, 815, -1, 779, 786, 823, -1, 778, 815, 814, -1, 815, 778, 779, -1, 777, 814, 812, -1, 814, 777, 778, -1, 38, 812, 813, -1, 812, 38, 777, -1, 39, 813, 822, -1, 813, 39, 38, -1, 822, 820, 784, -1, 784, 39, 822, -1, 782, 820, 818, -1, 782, 784, 820, -1, 818, 816, 780, -1, 818, 780, 782, -1, 816, 817, 781, -1, 816, 781, 780, -1, 817, 819, 783, -1, 817, 783, 781, -1, 819, 821, 785, -1, 785, 783, 819, -1, 821, 852, 785, -1, 853, 785, 852, -1, 852, 854, 853, -1, 852, 855, 854, -1, 855, 10, 854, -1, 10, 855, 835, -1, 11, 835, 834, -1, 835, 11, 10, -1, 834, 836, 11, -1, 847, 11, 836, -1, 848, 836, 838, -1, 848, 847, 836, -1, 849, 838, 840, -1, 849, 848, 838, -1, 840, 842, 850, -1, 840, 850, 849, -1, 842, 843, 851, -1, 851, 850, 842, -1, 843, 28, 17, -1, 843, 17, 851, -1, 28, 27, 2, -1, 2, 17, 28, -1, 2, 27, 23, -1, 0, 2, 23, -1, 856, 10, 12, -1, 856, 12, 775, -1, 785, 856, 775, -1, 785, 853, 856, -1, 853, 854, 856, -1, 854, 10, 856, -1, 857, 830, 835, -1, 857, 810, 830, -1, 821, 810, 857, -1, 852, 821, 857, -1, 855, 852, 857, -1, 835, 855, 857, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 10.500 0.001 63.772 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 10.500 0.001 -104.772 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 10.500 -84.270 -20.500 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 10.500 84.273 -20.500 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -73.772 0.001 -20.500 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 94.772 0.001 -20.500 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_KNEE_P.wrl000066400000000000000000001544031207742442300230540ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 27.586 39.900 81.338 center 0.000 0.000 0.000 #translation -5.793 4.050 25.031 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.50 0.50 0.50 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 7.871 -14.900 1.428, 8.000 -15.100 -0.000, 8.000 -14.900 -0.000, 7.789 -15.100 1.826, 7.490 -14.900 2.811, 7.167 -15.100 3.555, 6.868 -14.900 4.103, 6.166 -15.100 5.097, 6.025 -14.900 5.264, 4.840 -15.100 6.370, 4.988 -14.900 6.255, 3.791 -14.900 7.045, 3.258 -15.100 7.306, 2.472 -14.900 7.608, 1.074 -14.900 7.928, 1.505 -15.100 7.857, -0.359 -14.900 7.992, -0.328 -15.100 7.993, -0.328 -17.700 7.993, -1.780 -14.900 7.799, -2.129 -17.700 7.712, -3.144 -14.900 7.356, -3.819 -17.700 7.030, -4.407 -14.900 6.677, -5.310 -17.700 5.983, -5.529 -14.900 5.782, -6.527 -17.700 4.626, -6.472 -14.900 4.702, -7.208 -14.900 3.471, -7.404 -17.700 3.030, -7.712 -14.900 2.128, -7.898 -17.700 1.275, -7.968 -14.900 0.717, -7.981 -17.700 -0.545, -7.968 -14.900 -0.717, -7.712 -14.900 -2.128, -7.651 -17.700 -2.337, -7.208 -14.900 -3.471, -6.924 -17.700 -4.008, -6.924 -15.100 -4.008, -6.472 -14.900 -4.702, -5.529 -14.900 -5.782, -5.652 -15.100 -5.662, -4.407 -14.900 -6.677, -3.144 -14.900 -7.356, -3.996 -15.100 -6.930, -1.780 -14.900 -7.799, -2.068 -15.100 -7.728, -0.359 -14.900 -7.992, -0.000 -15.100 -8.000, 1.074 -14.900 -7.928, 1.780 -15.100 -7.799, 2.472 -14.900 -7.608, 3.471 -15.100 -7.208, 3.791 -14.900 -7.045, 4.988 -15.100 -6.255, 4.988 -14.900 -6.255, 6.025 -14.900 -5.264, 6.255 -15.100 -4.988, 6.868 -14.900 -4.103, 7.208 -15.100 -3.471, 7.490 -14.900 -2.811, 7.799 -15.100 -1.780, 7.871 -14.900 -1.428, 18.604 -15.100 -5.978, 18.600 -15.100 -6.026, 18.600 -15.100 -5.994, 10.157 -15.100 -5.257, 6.255 -15.100 -4.988, 7.208 -15.100 -3.471, 10.104 -15.100 -6.756, 19.151 -15.100 -5.946, 18.601 -15.100 -5.978, -6.924 -15.100 -4.008, -3.991 -15.100 -13.375, -4.095 -15.100 -14.565, 7.799 -15.100 -1.780, -5.652 -15.100 -5.662, -3.996 -15.100 -6.930, 17.187 -15.100 -1.001, 17.109 -15.100 -5.499, -2.068 -15.100 -7.728, 8.000 -15.100 -0.000, 7.789 -15.100 1.826, 7.167 -15.100 3.555, -0.000 -15.100 -8.000, 1.780 -15.100 -7.799, 18.687 -15.100 -1.027, 19.210 -15.100 -5.958, 19.586 -15.100 15.638, 16.855 -15.100 -6.991, 15.906 -15.100 -11.457, 14.154 -15.100 -11.396, 6.166 -15.100 5.097, 4.840 -15.100 6.370, 3.258 -15.100 7.306, 1.505 -15.100 7.857, -0.328 -15.100 7.993, 16.648 -15.100 -15.181, 18.594 -15.100 -6.026, 17.178 -15.100 -15.227, 15.142 -15.100 -15.049, 14.102 -15.100 -12.895, 3.471 -15.100 -7.208, 15.589 -15.100 -12.947, 4.988 -15.100 -6.255, -2.828 -14.900 -2.828, -3.464 -14.900 -2.000, -2.000 -14.900 -3.464, -3.864 -14.900 -1.035, -1.035 -14.900 -3.864, -4.000 -14.900 -0.000, -0.000 -14.900 -4.000, -3.864 -14.900 1.035, 1.035 -14.900 -3.864, -3.464 -14.900 2.000, 2.000 -14.900 -3.464, -2.828 -14.900 2.828, 2.828 -14.900 -2.828, 3.464 -14.900 -2.000, -2.000 -14.900 3.464, -1.035 -14.900 3.864, 3.864 -14.900 -1.035, 4.000 -14.900 0.000, -0.000 -14.900 4.000, 1.035 -14.900 3.864, 3.864 -14.900 1.035, 2.000 -14.900 3.464, 3.464 -14.900 2.000, 2.828 -14.900 2.828, -4.686 -17.700 6.321, -4.686 -17.900 6.321, 19.586 -17.900 15.638, 7.450 -16.500 10.979, 19.586 -15.100 15.638, -7.188 -17.700 -3.020, -7.188 -17.900 -3.020, -4.095 -17.900 -14.565, -4.095 -15.100 -14.565, -5.642 -16.500 -8.792, 10.104 13.100 -6.756, 16.855 13.100 -6.991, 10.157 13.100 -5.257, 17.109 13.100 -5.499, 17.187 13.100 -1.001, 18.687 13.100 -1.027, 18.601 13.100 -5.978, 18.604 13.100 -5.978, 18.600 13.100 -5.994, 18.600 13.100 -6.026, 18.594 13.100 -6.026, 17.330 -1.000 -11.969, 16.067 -15.100 -17.912, 16.067 13.100 -17.912, 16.648 13.100 -15.181, 16.550 -15.100 -18.181, 14.600 -15.100 -17.600, -3.219 -15.100 -17.836, 6.526 -15.100 -15.782, 7.659 -17.700 -60.010, 7.542 -15.100 -60.560, 7.659 -15.100 -60.010, 7.542 -17.900 -60.560, 7.937 -17.700 -58.705, 17.178 -17.900 -15.227, 16.550 -15.100 -18.181, 17.178 -15.100 -15.227, 16.550 -17.900 -18.181, 19.151 -15.100 -5.946, 19.151 -17.900 -5.946, 13.347 -16.500 -33.253, 7.937 -15.100 -58.705, 19.210 -17.900 -5.958, 19.210 -15.100 -5.958, 19.210 -17.900 -5.958, 19.210 -15.100 -5.958, 15.589 13.100 -12.947, 15.094 -1.000 -15.274, 15.142 13.100 -15.049, 14.600 13.100 -17.600, 14.102 13.100 -12.895, 14.154 13.100 -11.396, 15.906 13.100 -11.457, -3.464 -17.900 2.000, 3.464 -17.900 -2.000, -2.828 -17.900 2.828, 2.828 -17.900 -2.828, 3.864 -17.900 -1.035, -3.864 -17.900 1.035, 4.000 -17.900 0.000, 3.864 -17.900 1.035, -4.000 -17.900 -0.000, 3.464 -17.900 2.000, -3.864 -17.900 -1.035, 2.828 -17.900 2.828, -3.464 -17.900 -2.000, 2.000 -17.900 3.464, -2.828 -17.900 -2.828, 1.035 -17.900 3.864, -2.000 -17.900 -3.464, -0.000 -17.900 4.000, -1.035 -17.900 -3.864, -1.035 -17.900 3.864, -0.000 -17.900 -4.000, 1.035 -17.900 -3.864, -2.000 -17.900 3.464, 2.000 -17.900 -3.464, 10.157 -17.900 -5.257, 17.187 -17.900 -1.001, 6.096 -17.900 0.420, 10.104 -17.900 -6.756, 14.154 -17.900 -11.396, 16.855 -17.900 -6.991, 15.906 -17.900 -11.457, -3.991 -17.900 -13.375, 18.600 -17.900 -5.994, 18.600 -17.900 -6.026, 18.604 -17.900 -5.978, 14.102 -17.900 -12.895, 15.142 -17.900 -15.049, 15.589 -17.900 -12.947, 18.601 -17.900 -5.978, 19.151 -17.900 -5.946, 17.109 -17.900 -5.499, 18.687 -17.900 -1.027, 18.594 -17.900 -6.026, 16.648 -17.900 -15.181, 17.178 -17.900 -15.227, -7.948 -15.100 -58.608, -8.000 -15.100 -59.200, -7.948 -17.700 -58.608, -8.000 -17.900 -59.200, -7.670 -17.700 -55.425, -4.095 -17.900 -14.565, -4.095 -15.100 -14.565, -6.047 -16.500 -36.883, -7.670 -15.100 -55.425, 7.208 13.100 -3.471, 6.255 13.100 -4.988, 19.151 13.100 -5.946, -4.095 13.100 -14.565, -3.991 13.100 -13.375, -6.924 13.100 -4.008, 7.799 13.100 -1.780, -5.652 13.100 -5.662, -3.996 13.100 -6.930, -2.068 13.100 -7.728, 8.000 13.100 -0.000, -0.000 13.100 -8.000, 7.789 13.100 1.826, 7.167 13.100 3.555, 1.780 13.100 -7.799, 19.210 13.100 -5.958, 19.586 13.100 15.638, 6.166 13.100 5.097, 4.840 13.100 6.370, 3.258 13.100 7.306, 1.505 13.100 7.857, -0.328 13.100 7.993, 17.178 13.100 -15.227, 3.471 13.100 -7.208, 4.988 13.100 -6.255, 16.550 13.100 -18.181, -3.219 13.100 -17.836, 6.526 13.100 -15.782, 7.937 -15.100 -58.705, 7.972 -15.100 -58.371, 7.993 -15.100 -58.036, 8.000 -15.100 -57.700, 7.775 -15.100 -55.816, 7.112 -15.100 -54.037, 4.436 -15.100 -36.855, 6.050 -15.100 -52.465, 4.646 -15.100 -51.188, 2.982 -15.100 -50.276, 1.149 -15.100 -49.783, -0.748 -15.100 -49.735, -2.603 -15.100 -50.135, -4.311 -15.100 -50.961, -5.777 -15.100 -52.166, -6.918 -15.100 -53.683, -7.670 -15.100 -55.425, 7.540 -15.100 -60.374, 7.474 -15.100 -60.554, 7.602 -15.100 -60.193, 3.142 -17.900 -60.175, 7.474 -17.700 -60.554, 3.142 -17.700 -60.175, -3.419 -17.900 -55.624, -6.000 -17.900 -42.000, 2.847 -17.900 -54.890, 3.473 -17.900 -55.715, 6.253 -17.900 -43.072, 2.031 -17.900 -54.254, 1.078 -17.900 -53.848, 0.053 -17.900 -53.700, -3.524 -17.900 -59.592, -3.892 -17.900 -58.624, -3.722 -17.900 -15.958, -3.999 -17.900 -57.594, -3.837 -17.900 -56.571, 8.443 -17.900 -18.039, 3.866 -17.900 -56.673, 4.000 -17.900 -57.700, 3.611 -17.900 -59.421, 3.902 -17.900 -58.582, -0.975 -17.900 -53.821, -1.938 -17.900 -54.201, -2.771 -17.900 -54.816, -3.664 -17.900 -15.302, 8.589 -17.900 -16.374, 14.600 -17.900 -17.600, 16.067 -17.900 -17.912, 6.526 -17.900 -15.782, 7.871 -14.900 -56.272, 8.000 -15.100 -57.700, 8.000 -14.900 -57.700, 7.775 -15.100 -55.816, 7.490 -14.900 -54.889, 7.112 -15.100 -54.037, 6.868 -14.900 -53.597, 6.050 -15.100 -52.465, 6.025 -14.900 -52.436, 4.988 -14.900 -51.445, 4.646 -15.100 -51.188, 3.791 -14.900 -50.655, 2.472 -14.900 -50.092, 2.982 -15.100 -50.276, 1.149 -15.100 -49.783, 1.074 -14.900 -49.772, -0.359 -14.900 -49.708, -0.748 -15.100 -49.735, -1.780 -14.900 -49.901, -3.144 -14.900 -50.344, -2.603 -15.100 -50.135, -4.311 -15.100 -50.961, -4.407 -14.900 -51.023, -5.529 -14.900 -51.918, -5.777 -15.100 -52.166, -6.472 -14.900 -52.998, -7.208 -14.900 -54.229, -6.918 -15.100 -53.683, -7.712 -14.900 -55.572, -7.905 -17.700 -56.471, -7.968 -14.900 -56.983, -7.998 -17.700 -57.538, -7.968 -14.900 -58.417, -7.923 -15.100 -58.811, -7.892 -15.100 -59.012, -7.856 -15.100 -59.213, -7.712 -14.900 -59.828, -7.856 -17.700 -59.213, -7.287 -17.700 -61.001, -7.208 -14.900 -61.171, -6.472 -14.900 -62.402, -6.317 -17.700 -62.608, -5.529 -14.900 -63.482, -5.000 -17.700 -63.945, -4.407 -14.900 -64.377, -3.407 -17.700 -64.938, -3.144 -14.900 -65.056, -1.627 -17.700 -65.533, -1.780 -14.900 -65.499, -0.359 -14.900 -65.692, 0.242 -17.700 -65.696, 1.074 -14.900 -65.628, 2.099 -17.700 -65.420, 2.472 -14.900 -65.308, 3.840 -17.700 -64.718, 3.791 -14.900 -64.745, 4.988 -14.900 -63.955, 5.369 -17.700 -63.631, 6.025 -14.900 -62.964, 6.603 -17.700 -62.216, 6.868 -14.900 -61.803, 7.490 -14.900 -60.511, 7.776 -17.700 -59.580, 7.871 -14.900 -59.128, 7.868 -17.700 -59.145, 7.972 -15.100 -58.371, 7.993 -15.100 -58.036, 10.104 -18.000 -6.756, 16.855 -18.000 -6.991, 10.157 -18.000 -5.257, 17.109 -18.000 -5.499, 17.187 -18.000 -1.001, 18.687 -18.000 -1.027, 18.601 -18.000 -5.978, 18.604 -18.000 -5.978, 18.600 -18.000 -5.994, 18.600 -18.000 -6.026, 18.594 -18.000 -6.026, 16.067 -18.000 -17.912, 17.330 -17.950 -11.969, 15.589 -18.000 -12.947, 14.600 -18.000 -17.600, 15.094 -17.950 -15.274, 14.102 -18.000 -12.895, 14.154 -18.000 -11.396, 15.906 -18.000 -11.457, -7.856 -17.700 -59.213, -7.856 -15.100 -59.213, -3.524 -17.700 -59.592, 7.659 13.100 -60.010, 7.542 13.100 -60.560, 7.659 15.700 -60.010, 7.542 15.900 -60.560, 7.937 15.700 -58.705, 17.178 15.900 -15.227, 16.550 15.900 -18.181, 19.151 15.900 -5.946, 7.937 13.100 -58.705, 13.347 14.500 -33.253, 19.210 15.900 -5.958, 19.586 15.900 15.638, -4.686 15.900 6.321, -4.686 15.700 6.321, -0.328 15.700 7.993, 7.450 14.500 10.979, 8.000 12.900 -0.000, 7.799 12.900 1.780, 7.208 12.900 3.471, 6.255 12.900 4.988, 4.988 12.900 6.255, 3.471 12.900 7.208, 1.780 12.900 7.799, -0.000 12.900 8.000, -1.780 12.900 7.799, -2.129 15.700 7.712, -3.471 12.900 7.208, -3.819 15.700 7.030, -4.988 12.900 6.255, -5.310 15.700 5.983, -6.255 12.900 4.988, -6.527 15.700 4.626, -7.208 12.900 3.471, -7.404 15.700 3.030, -7.799 12.900 1.780, -7.898 15.700 1.275, -8.000 12.900 -0.000, -7.981 15.700 -0.545, -7.799 12.900 -1.780, -7.651 15.700 -2.337, -7.208 12.900 -3.471, -6.924 15.700 -4.008, -6.255 12.900 -4.988, -4.988 12.900 -6.255, -3.471 12.900 -7.208, -1.780 12.900 -7.799, 0.000 12.900 -8.000, 1.780 12.900 -7.799, 3.471 12.900 -7.208, 4.988 12.900 -6.255, 6.255 12.900 -4.988, 7.208 12.900 -3.471, 7.799 12.900 -1.780, -7.188 15.900 -3.020, -7.188 15.700 -3.020, -4.095 15.900 -14.565, -5.642 14.500 -8.792, 7.972 13.100 -58.371, 7.993 13.100 -58.036, 8.000 13.100 -57.700, 7.775 13.100 -55.816, 7.112 13.100 -54.037, 4.436 13.100 -36.855, 6.050 13.100 -52.465, 4.646 13.100 -51.188, 2.982 13.100 -50.276, 1.149 13.100 -49.783, -0.748 13.100 -49.735, -2.603 13.100 -50.135, -4.311 13.100 -50.961, -5.777 13.100 -52.166, -6.918 13.100 -53.683, -7.670 13.100 -55.425, -0.191 -17.700 -62.454, 0.217 -17.700 -61.694, -0.907 -17.700 -61.596, 1.324 -17.700 -61.474, 2.099 -17.700 -65.420, -2.856 -17.700 -60.501, -1.960 -17.700 -61.187, -7.287 -17.700 -61.001, 2.326 -17.700 -60.954, 3.864 -14.900 -56.665, 4.000 -14.900 -57.700, 3.464 -14.900 -55.700, 2.828 -14.900 -54.872, 2.000 -14.900 -54.236, 1.035 -14.900 -53.836, -0.000 -14.900 -53.700, -1.035 -14.900 -53.836, -2.000 -14.900 -54.236, -2.828 -14.900 -54.872, -3.464 -14.900 -55.700, -3.864 -14.900 -56.665, -4.000 -14.900 -57.700, -3.864 -14.900 -58.735, -3.464 -14.900 -59.700, -2.828 -14.900 -60.528, -2.000 -14.900 -61.164, -1.035 -14.900 -61.564, -0.000 -14.900 -61.700, 1.035 -14.900 -61.564, 2.000 -14.900 -61.164, 2.828 -14.900 -60.528, 3.464 -14.900 -59.700, 3.864 -14.900 -58.735, 8.589 -24.000 -16.374, 7.421 -20.950 -29.723, 6.253 -24.000 -43.072, -6.000 -24.000 -42.000, -3.664 -24.000 -15.302, -4.832 -20.950 -28.651, -5.529 -14.900 -63.482, -6.472 -14.900 -62.402, -7.968 -14.900 -56.983, -4.407 -14.900 -51.023, -3.144 -14.900 -50.344, 1.074 -14.900 -49.772, 7.871 -14.900 -56.272, 7.490 -14.900 -54.889, 17.103 -18.000 -5.824, 13.609 -18.000 -6.128, 17.152 -18.000 -5.501, 17.146 -18.000 -5.669, 17.894 -18.000 -3.489, 17.100 -18.000 -13.000, 17.152 -18.000 -11.501, 17.100 -18.000 -7.000, 17.135 -18.000 -6.001, 15.004 -18.000 -12.176, 7.474 13.100 -60.554, 7.540 13.100 -60.374, 7.602 13.100 -60.193, 7.474 15.700 -60.554, 3.142 15.900 -60.175, 3.142 15.700 -60.175, -3.219 15.900 -17.836, -8.000 15.900 -59.200, -3.892 15.900 -58.624, -3.524 15.900 -59.592, 4.354 15.900 -37.792, 3.473 15.900 -55.715, 2.847 15.900 -54.890, 2.031 15.900 -54.254, -3.999 15.900 -57.594, 1.078 15.900 -53.848, 0.053 15.900 -53.700, -0.975 15.900 -53.821, -1.938 15.900 -54.201, -2.771 15.900 -54.816, -3.837 15.900 -56.571, -3.419 15.900 -55.624, 3.611 15.900 -59.421, 3.902 15.900 -58.582, 4.000 15.900 -57.700, 3.866 15.900 -56.673, 6.526 15.900 -15.782, -3.991 15.900 -13.375, -4.095 15.900 -14.565, -3.991 15.900 -13.375, 2.000 15.900 -3.464, -1.035 15.900 3.864, -2.000 15.900 3.464, -0.000 15.900 4.000, -3.464 15.900 -2.000, -2.828 15.900 -2.828, -2.000 15.900 -3.464, -3.864 15.900 -1.035, -1.035 15.900 -3.864, 1.035 15.900 3.864, -4.000 15.900 -0.000, -0.000 15.900 -4.000, -3.864 15.900 1.035, 2.828 15.900 -2.828, 3.464 15.900 -2.000, 1.035 15.900 -3.864, 2.828 15.900 2.828, 2.000 15.900 3.464, 6.096 15.900 0.420, 3.864 15.900 -1.035, 4.000 15.900 0.000, 3.864 15.900 1.035, 3.464 15.900 2.000, -2.828 15.900 2.828, -3.464 15.900 2.000, 8.000 12.900 -57.700, 7.799 12.900 -55.920, 7.775 13.100 -55.816, 7.208 12.900 -54.229, 6.255 12.900 -52.712, 4.988 12.900 -51.445, 3.471 12.900 -50.492, 2.982 13.100 -50.276, 1.780 12.900 -49.901, -0.000 12.900 -49.700, -1.780 12.900 -49.901, -3.471 12.900 -50.492, -4.988 12.900 -51.445, -5.777 13.100 -52.166, -6.255 12.900 -52.712, -6.918 13.100 -53.683, -7.208 12.900 -54.229, -7.799 12.900 -55.920, -7.670 15.700 -55.425, -7.905 15.700 -56.471, -8.000 12.900 -57.700, -7.998 15.700 -57.538, -7.948 13.100 -58.608, -7.948 15.700 -58.608, -7.799 12.900 -59.480, -7.923 13.100 -58.811, -7.892 13.100 -59.012, -7.856 13.100 -59.213, -7.856 15.700 -59.213, -7.208 12.900 -61.171, -7.287 15.700 -61.001, -6.255 12.900 -62.688, -6.317 15.700 -62.608, -4.988 12.900 -63.955, -5.000 15.700 -63.945, -3.471 12.900 -64.908, -3.407 15.700 -64.938, -1.780 12.900 -65.499, -1.627 15.700 -65.533, 0.000 12.900 -65.700, 0.242 15.700 -65.696, 1.780 12.900 -65.499, 2.099 15.700 -65.420, 3.471 12.900 -64.908, 3.840 15.700 -64.718, 4.988 12.900 -63.955, 5.369 15.700 -63.631, 6.255 12.900 -62.688, 6.603 15.700 -62.216, 7.208 12.900 -61.171, 7.799 12.900 -59.480, 7.776 15.700 -59.580, 7.868 15.700 -59.145, 7.993 13.100 -58.036, -2.828 12.900 -2.828, -3.464 12.900 -2.000, -2.000 12.900 -3.464, -3.864 12.900 -1.035, -1.035 12.900 -3.864, -4.000 12.900 -0.000, -0.000 12.900 -4.000, -3.864 12.900 1.035, 1.035 12.900 -3.864, -3.464 12.900 2.000, 2.000 12.900 -3.464, -2.828 12.900 2.828, 2.828 12.900 -2.828, -2.000 12.900 3.464, 3.464 12.900 -2.000, -1.035 12.900 3.864, 3.864 12.900 -1.035, -0.000 12.900 4.000, 4.000 12.900 0.000, 1.035 12.900 3.864, 3.864 12.900 1.035, 2.000 12.900 3.464, 3.464 12.900 2.000, 2.828 12.900 2.828, -8.000 13.100 -59.200, -6.047 14.500 -36.883, -4.095 13.100 -14.565, 6.253 -24.000 -43.072, 8.589 -24.000 -16.374, -3.664 -24.000 -15.302, 16.998 -18.000 -6.408, 17.100 -18.000 -6.000, 17.125 -18.000 -5.662, 17.137 -18.000 -5.667, 16.371 -18.000 -12.224, 2.326 15.700 -60.954, -0.907 15.700 -61.596, 0.217 15.700 -61.694, -0.191 15.700 -62.454, 1.324 15.700 -61.474, 2.099 15.700 -65.420, -2.856 15.700 -60.501, -1.960 15.700 -61.187, -7.287 15.700 -61.001, -7.856 15.700 -59.213, -3.524 15.700 -59.592, 4.000 12.900 -57.700, 3.864 12.900 -56.665, 3.464 12.900 -55.700, 2.828 12.900 -54.872, 2.000 12.900 -54.236, 1.035 12.900 -53.836, -0.000 12.900 -53.700, -1.035 12.900 -53.836, -2.000 12.900 -54.236, -2.828 12.900 -54.872, -3.464 12.900 -55.700, -3.864 12.900 -56.665, -4.000 12.900 -57.700, -3.864 12.900 -58.735, -3.464 12.900 -59.700, -2.828 12.900 -60.528, -2.000 12.900 -61.164, -1.035 12.900 -61.564, -0.000 12.900 -61.700, 1.035 12.900 -61.564, 2.000 12.900 -61.164, 2.828 12.900 -60.528, 3.464 12.900 -59.700, 3.864 12.900 -58.735, -7.856 13.100 -59.213, -6.255 12.900 -62.688, 6.255 12.900 -62.688, 3.471 12.900 -50.492, 17.122 -18.000 -5.833 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 3, 1, 0, -1, 4, 3, 0, -1, 5, 3, 4, -1, 6, 5, 4, -1, 7, 5, 6, -1, 8, 7, 6, -1, 9, 8, 10, -1, 9, 7, 8, -1, 11, 9, 10, -1, 12, 9, 11, -1, 13, 12, 11, -1, 14, 15, 13, -1, 16, 17, 14, -1, 18, 17, 16, -1, 19, 18, 16, -1, 20, 18, 19, -1, 21, 20, 19, -1, 22, 20, 21, -1, 23, 22, 21, -1, 24, 22, 23, -1, 25, 24, 23, -1, 26, 24, 25, -1, 26, 25, 27, -1, 28, 26, 27, -1, 29, 26, 28, -1, 30, 29, 28, -1, 31, 29, 30, -1, 32, 31, 30, -1, 33, 32, 34, -1, 33, 31, 32, -1, 35, 33, 34, -1, 36, 33, 35, -1, 37, 36, 35, -1, 38, 36, 37, -1, 38, 37, 39, -1, 40, 39, 37, -1, 41, 42, 40, -1, 43, 42, 41, -1, 44, 45, 43, -1, 46, 47, 44, -1, 48, 47, 46, -1, 49, 47, 48, -1, 50, 49, 48, -1, 51, 49, 50, -1, 52, 51, 50, -1, 53, 51, 52, -1, 54, 53, 52, -1, 55, 54, 56, -1, 55, 53, 54, -1, 57, 55, 56, -1, 58, 55, 57, -1, 59, 58, 57, -1, 60, 58, 59, -1, 61, 60, 59, -1, 62, 60, 61, -1, 63, 62, 61, -1, 1, 62, 63, -1, 1, 63, 2, -1, 42, 39, 40, -1, 45, 42, 43, -1, 47, 45, 44, -1, 15, 12, 13, -1, 17, 15, 14, -1, 64, 65, 66, -1, 67, 68, 69, -1, 67, 70, 68, -1, 71, 65, 64, -1, 71, 64, 72, -1, 73, 74, 75, -1, 76, 67, 69, -1, 73, 77, 74, -1, 78, 74, 77, -1, 79, 80, 67, -1, 81, 74, 78, -1, 82, 67, 76, -1, 79, 82, 83, -1, 79, 83, 84, -1, 85, 74, 81, -1, 79, 67, 82, -1, 86, 74, 85, -1, 87, 88, 71, -1, 87, 71, 72, -1, 89, 87, 79, -1, 90, 91, 92, -1, 89, 84, 93, -1, 89, 93, 94, -1, 89, 94, 95, -1, 90, 92, 70, -1, 89, 95, 96, -1, 89, 96, 97, -1, 89, 79, 84, -1, 98, 99, 65, -1, 98, 65, 100, -1, 71, 100, 65, -1, 89, 88, 87, -1, 101, 74, 102, -1, 102, 74, 86, -1, 102, 86, 103, -1, 104, 101, 102, -1, 70, 103, 105, -1, 70, 105, 68, -1, 70, 92, 102, -1, 70, 102, 103, -1, 41, 40, 106, -1, 43, 41, 106, -1, 40, 37, 107, -1, 106, 40, 107, -1, 44, 43, 108, -1, 43, 106, 108, -1, 35, 34, 109, -1, 37, 35, 109, -1, 107, 37, 109, -1, 46, 44, 110, -1, 48, 46, 110, -1, 44, 108, 110, -1, 34, 32, 111, -1, 109, 34, 111, -1, 50, 48, 112, -1, 48, 110, 112, -1, 30, 28, 113, -1, 32, 30, 113, -1, 111, 32, 113, -1, 50, 112, 114, -1, 52, 50, 114, -1, 113, 28, 115, -1, 28, 27, 115, -1, 52, 114, 116, -1, 54, 52, 116, -1, 54, 116, 56, -1, 115, 27, 117, -1, 56, 116, 118, -1, 117, 27, 25, -1, 56, 118, 57, -1, 57, 118, 119, -1, 120, 117, 23, -1, 117, 25, 23, -1, 57, 119, 59, -1, 121, 120, 21, -1, 120, 23, 21, -1, 119, 122, 61, -1, 59, 119, 61, -1, 121, 21, 19, -1, 122, 123, 63, -1, 61, 122, 63, -1, 124, 121, 16, -1, 121, 19, 16, -1, 63, 123, 2, -1, 125, 124, 14, -1, 124, 16, 14, -1, 123, 126, 0, -1, 2, 123, 0, -1, 127, 125, 13, -1, 125, 14, 13, -1, 126, 128, 4, -1, 0, 126, 4, -1, 127, 13, 11, -1, 4, 128, 6, -1, 129, 127, 10, -1, 127, 11, 10, -1, 128, 129, 8, -1, 129, 10, 8, -1, 6, 128, 8, -1, 18, 130, 131, -1, 132, 18, 131, -1, 133, 134, 17, -1, 133, 132, 134, -1, 133, 17, 18, -1, 133, 18, 132, -1, 36, 38, 135, -1, 33, 36, 135, -1, 31, 33, 135, -1, 29, 31, 135, -1, 130, 24, 26, -1, 130, 26, 29, -1, 130, 29, 135, -1, 22, 24, 130, -1, 20, 22, 130, -1, 18, 20, 130, -1, 135, 38, 136, -1, 38, 137, 136, -1, 39, 138, 139, -1, 138, 137, 139, -1, 38, 39, 139, -1, 137, 38, 139, -1, 140, 141, 90, -1, 140, 90, 70, -1, 140, 70, 67, -1, 142, 140, 67, -1, 80, 143, 142, -1, 67, 80, 142, -1, 144, 143, 80, -1, 144, 80, 79, -1, 144, 79, 87, -1, 145, 144, 87, -1, 72, 146, 145, -1, 87, 72, 145, -1, 146, 72, 64, -1, 147, 146, 64, -1, 66, 148, 147, -1, 64, 66, 147, -1, 65, 149, 148, -1, 66, 65, 148, -1, 65, 99, 150, -1, 65, 150, 149, -1, 151, 152, 153, -1, 151, 98, 152, -1, 151, 99, 98, -1, 151, 150, 99, -1, 151, 154, 150, -1, 151, 153, 154, -1, 155, 156, 152, -1, 100, 152, 98, -1, 100, 155, 152, -1, 155, 157, 156, -1, 158, 74, 101, -1, 158, 75, 74, -1, 158, 157, 75, -1, 158, 101, 156, -1, 158, 156, 157, -1, 159, 160, 161, -1, 159, 162, 160, -1, 163, 162, 159, -1, 164, 165, 166, -1, 164, 167, 165, -1, 168, 164, 166, -1, 169, 164, 168, -1, 167, 162, 163, -1, 170, 171, 165, -1, 170, 163, 171, -1, 170, 167, 163, -1, 170, 165, 167, -1, 172, 169, 168, -1, 172, 168, 173, -1, 134, 174, 175, -1, 132, 174, 134, -1, 104, 176, 177, -1, 176, 178, 177, -1, 178, 179, 177, -1, 179, 156, 177, -1, 156, 101, 177, -1, 101, 104, 177, -1, 180, 176, 104, -1, 180, 104, 102, -1, 180, 102, 92, -1, 181, 180, 92, -1, 91, 182, 181, -1, 92, 91, 181, -1, 141, 182, 91, -1, 141, 91, 90, -1, 115, 117, 183, -1, 119, 118, 184, -1, 117, 185, 183, -1, 118, 186, 184, -1, 115, 183, 113, -1, 122, 119, 187, -1, 113, 183, 188, -1, 123, 189, 190, -1, 119, 184, 187, -1, 126, 123, 190, -1, 122, 187, 123, -1, 113, 188, 191, -1, 126, 190, 128, -1, 111, 113, 191, -1, 123, 187, 189, -1, 128, 190, 192, -1, 111, 191, 193, -1, 109, 111, 193, -1, 129, 128, 194, -1, 128, 192, 194, -1, 109, 193, 195, -1, 107, 109, 195, -1, 127, 129, 196, -1, 129, 194, 196, -1, 107, 195, 197, -1, 106, 107, 197, -1, 125, 127, 198, -1, 106, 197, 199, -1, 127, 196, 198, -1, 108, 106, 199, -1, 108, 199, 110, -1, 124, 125, 200, -1, 125, 198, 200, -1, 110, 199, 201, -1, 110, 201, 112, -1, 121, 124, 202, -1, 112, 201, 203, -1, 124, 200, 202, -1, 112, 203, 114, -1, 114, 203, 204, -1, 120, 121, 205, -1, 114, 204, 116, -1, 121, 202, 205, -1, 116, 204, 206, -1, 120, 205, 117, -1, 117, 205, 185, -1, 118, 116, 186, -1, 116, 206, 186, -1, 207, 208, 209, -1, 136, 188, 131, -1, 210, 211, 212, -1, 211, 213, 212, -1, 137, 214, 136, -1, 136, 197, 195, -1, 136, 214, 199, -1, 197, 136, 199, -1, 131, 205, 202, -1, 136, 195, 193, -1, 199, 214, 201, -1, 215, 216, 217, -1, 136, 193, 191, -1, 218, 219, 220, -1, 221, 217, 222, -1, 201, 214, 203, -1, 217, 216, 222, -1, 218, 211, 210, -1, 206, 204, 210, -1, 186, 206, 210, -1, 136, 191, 188, -1, 204, 214, 210, -1, 214, 218, 210, -1, 207, 223, 208, -1, 203, 214, 204, -1, 131, 202, 200, -1, 131, 200, 198, -1, 222, 174, 224, -1, 221, 222, 224, -1, 131, 198, 132, -1, 208, 224, 132, -1, 196, 194, 132, -1, 198, 196, 132, -1, 216, 225, 226, -1, 227, 216, 226, -1, 186, 210, 207, -1, 216, 227, 222, -1, 184, 186, 207, -1, 224, 174, 132, -1, 218, 214, 219, -1, 184, 207, 209, -1, 132, 194, 209, -1, 208, 132, 209, -1, 187, 184, 209, -1, 185, 205, 131, -1, 189, 187, 209, -1, 183, 185, 131, -1, 190, 189, 209, -1, 188, 183, 131, -1, 192, 190, 209, -1, 194, 192, 209, -1, 135, 136, 131, -1, 130, 135, 131, -1, 228, 229, 230, -1, 229, 231, 230, -1, 230, 231, 232, -1, 232, 231, 233, -1, 233, 234, 235, -1, 234, 236, 235, -1, 236, 232, 235, -1, 232, 233, 235, -1, 148, 149, 147, -1, 237, 238, 142, -1, 238, 140, 142, -1, 147, 149, 239, -1, 240, 241, 242, -1, 146, 147, 239, -1, 237, 142, 243, -1, 241, 244, 242, -1, 244, 241, 245, -1, 245, 241, 246, -1, 243, 142, 247, -1, 142, 143, 144, -1, 246, 241, 248, -1, 249, 247, 144, -1, 250, 249, 144, -1, 248, 241, 251, -1, 247, 142, 144, -1, 239, 252, 145, -1, 146, 239, 145, -1, 181, 182, 141, -1, 144, 145, 253, -1, 254, 250, 253, -1, 255, 254, 253, -1, 140, 181, 141, -1, 256, 255, 253, -1, 257, 256, 253, -1, 258, 257, 253, -1, 250, 144, 253, -1, 149, 150, 154, -1, 259, 149, 154, -1, 149, 259, 239, -1, 145, 252, 253, -1, 180, 241, 178, -1, 260, 251, 180, -1, 251, 241, 180, -1, 180, 178, 176, -1, 261, 260, 140, -1, 238, 261, 140, -1, 180, 181, 140, -1, 260, 180, 140, -1, 152, 156, 179, -1, 152, 179, 153, -1, 153, 179, 262, -1, 154, 153, 259, -1, 153, 262, 259, -1, 179, 263, 262, -1, 178, 241, 264, -1, 241, 240, 264, -1, 240, 263, 264, -1, 179, 178, 264, -1, 263, 179, 264, -1, 155, 265, 266, -1, 155, 266, 267, -1, 155, 267, 268, -1, 155, 268, 269, -1, 155, 269, 270, -1, 271, 157, 155, -1, 271, 270, 272, -1, 271, 272, 273, -1, 271, 273, 274, -1, 271, 274, 275, -1, 271, 275, 276, -1, 271, 276, 277, -1, 271, 277, 278, -1, 271, 278, 157, -1, 271, 155, 270, -1, 157, 278, 279, -1, 157, 279, 280, -1, 75, 280, 281, -1, 75, 157, 280, -1, 282, 160, 283, -1, 161, 160, 284, -1, 284, 160, 282, -1, 162, 285, 286, -1, 285, 287, 286, -1, 286, 283, 160, -1, 162, 286, 160, -1, 231, 288, 289, -1, 290, 291, 292, -1, 293, 290, 292, -1, 294, 293, 292, -1, 295, 294, 292, -1, 291, 162, 292, -1, 289, 295, 292, -1, 231, 296, 297, -1, 289, 298, 137, -1, 231, 297, 299, -1, 231, 299, 300, -1, 301, 292, 167, -1, 231, 300, 288, -1, 289, 137, 231, -1, 292, 162, 167, -1, 302, 303, 162, -1, 291, 302, 162, -1, 304, 285, 162, -1, 305, 304, 162, -1, 303, 305, 162, -1, 306, 295, 289, -1, 307, 306, 289, -1, 308, 307, 289, -1, 288, 308, 289, -1, 298, 309, 137, -1, 137, 309, 214, -1, 310, 301, 311, -1, 312, 311, 167, -1, 311, 301, 167, -1, 310, 311, 219, -1, 226, 312, 227, -1, 312, 167, 227, -1, 309, 310, 313, -1, 219, 214, 313, -1, 214, 309, 313, -1, 310, 219, 313, -1, 314, 315, 316, -1, 317, 315, 314, -1, 318, 317, 314, -1, 319, 317, 318, -1, 320, 319, 318, -1, 321, 319, 320, -1, 322, 321, 320, -1, 323, 321, 322, -1, 324, 321, 323, -1, 325, 324, 323, -1, 326, 327, 325, -1, 328, 327, 326, -1, 329, 328, 326, -1, 330, 328, 329, -1, 331, 328, 330, -1, 332, 331, 330, -1, 333, 334, 332, -1, 335, 334, 333, -1, 336, 335, 333, -1, 337, 335, 336, -1, 338, 335, 337, -1, 339, 338, 337, -1, 340, 341, 339, -1, 342, 236, 340, -1, 232, 236, 342, -1, 343, 232, 342, -1, 344, 343, 342, -1, 345, 343, 344, -1, 346, 345, 344, -1, 347, 228, 346, -1, 230, 345, 346, -1, 230, 346, 228, -1, 348, 347, 346, -1, 349, 348, 346, -1, 350, 351, 349, -1, 350, 349, 346, -1, 352, 350, 353, -1, 352, 351, 350, -1, 354, 352, 353, -1, 355, 352, 354, -1, 356, 355, 354, -1, 357, 355, 356, -1, 358, 357, 356, -1, 359, 357, 358, -1, 360, 359, 358, -1, 361, 360, 362, -1, 361, 359, 360, -1, 363, 361, 362, -1, 364, 361, 363, -1, 365, 364, 363, -1, 366, 364, 365, -1, 367, 366, 365, -1, 368, 366, 367, -1, 368, 367, 369, -1, 370, 368, 369, -1, 371, 368, 370, -1, 372, 371, 370, -1, 373, 371, 372, -1, 374, 373, 372, -1, 375, 283, 374, -1, 286, 373, 374, -1, 286, 374, 283, -1, 284, 282, 375, -1, 161, 284, 375, -1, 376, 159, 161, -1, 377, 161, 375, -1, 377, 376, 161, -1, 378, 376, 377, -1, 163, 378, 377, -1, 163, 377, 171, -1, 379, 171, 377, -1, 316, 380, 379, -1, 316, 379, 377, -1, 315, 380, 316, -1, 327, 324, 325, -1, 334, 331, 332, -1, 341, 338, 339, -1, 236, 341, 340, -1, 282, 283, 375, -1, 159, 376, 378, -1, 159, 378, 163, -1, 210, 212, 381, -1, 212, 382, 381, -1, 207, 381, 383, -1, 207, 210, 381, -1, 207, 384, 223, -1, 383, 384, 207, -1, 208, 223, 385, -1, 223, 384, 385, -1, 386, 208, 385, -1, 224, 208, 386, -1, 224, 387, 221, -1, 386, 387, 224, -1, 221, 387, 388, -1, 217, 221, 388, -1, 388, 215, 217, -1, 388, 389, 215, -1, 389, 216, 215, -1, 389, 390, 216, -1, 390, 391, 225, -1, 390, 225, 216, -1, 226, 392, 312, -1, 393, 225, 391, -1, 393, 226, 225, -1, 393, 391, 392, -1, 393, 392, 226, -1, 220, 219, 394, -1, 311, 395, 396, -1, 395, 394, 396, -1, 219, 311, 396, -1, 394, 219, 396, -1, 220, 394, 218, -1, 218, 394, 397, -1, 211, 397, 398, -1, 211, 218, 397, -1, 211, 399, 213, -1, 398, 399, 211, -1, 213, 399, 212, -1, 212, 399, 382, -1, 348, 349, 229, -1, 347, 348, 229, -1, 228, 347, 229, -1, 400, 231, 229, -1, 400, 229, 401, -1, 231, 400, 296, -1, 296, 400, 402, -1, 230, 232, 343, -1, 230, 343, 345, -1, 403, 404, 405, -1, 404, 406, 405, -1, 405, 406, 407, -1, 259, 262, 408, -1, 262, 409, 408, -1, 259, 408, 239, -1, 239, 408, 410, -1, 407, 406, 409, -1, 262, 411, 412, -1, 411, 407, 412, -1, 407, 409, 412, -1, 409, 262, 412, -1, 239, 410, 413, -1, 252, 239, 413, -1, 252, 413, 253, -1, 253, 413, 414, -1, 415, 416, 417, -1, 415, 417, 414, -1, 258, 253, 418, -1, 253, 414, 418, -1, 417, 258, 418, -1, 414, 417, 418, -1, 419, 247, 420, -1, 420, 247, 249, -1, 420, 249, 421, -1, 421, 249, 250, -1, 421, 250, 422, -1, 422, 250, 254, -1, 422, 254, 423, -1, 423, 254, 255, -1, 423, 255, 424, -1, 424, 255, 256, -1, 424, 256, 425, -1, 425, 256, 257, -1, 425, 257, 426, -1, 258, 417, 427, -1, 426, 258, 427, -1, 427, 417, 428, -1, 427, 428, 429, -1, 429, 428, 430, -1, 429, 430, 431, -1, 431, 430, 432, -1, 431, 432, 433, -1, 433, 432, 434, -1, 433, 434, 435, -1, 435, 434, 436, -1, 435, 436, 437, -1, 437, 436, 438, -1, 437, 438, 439, -1, 439, 438, 440, -1, 439, 440, 441, -1, 441, 440, 442, -1, 441, 442, 443, -1, 443, 442, 444, -1, 242, 443, 444, -1, 443, 242, 445, -1, 445, 244, 446, -1, 446, 245, 447, -1, 447, 246, 448, -1, 449, 448, 248, -1, 448, 246, 248, -1, 449, 248, 450, -1, 450, 248, 251, -1, 451, 450, 260, -1, 450, 251, 260, -1, 452, 451, 261, -1, 451, 260, 261, -1, 453, 452, 238, -1, 452, 261, 238, -1, 454, 453, 237, -1, 453, 238, 237, -1, 454, 237, 455, -1, 455, 237, 243, -1, 455, 243, 419, -1, 419, 243, 247, -1, 445, 242, 244, -1, 446, 244, 245, -1, 447, 245, 246, -1, 426, 257, 258, -1, 456, 444, 457, -1, 456, 458, 444, -1, 459, 240, 242, -1, 459, 458, 240, -1, 459, 242, 444, -1, 459, 444, 458, -1, 460, 411, 262, -1, 461, 460, 262, -1, 462, 461, 262, -1, 463, 462, 262, -1, 464, 463, 262, -1, 262, 263, 465, -1, 466, 464, 465, -1, 467, 466, 465, -1, 468, 467, 465, -1, 469, 468, 465, -1, 470, 469, 465, -1, 471, 470, 465, -1, 472, 471, 465, -1, 263, 472, 465, -1, 464, 262, 465, -1, 473, 472, 263, -1, 474, 473, 263, -1, 475, 474, 240, -1, 474, 263, 240, -1, 373, 287, 371, -1, 286, 287, 373, -1, 476, 477, 478, -1, 476, 479, 477, -1, 476, 359, 361, -1, 476, 361, 364, -1, 476, 364, 480, -1, 476, 478, 359, -1, 476, 480, 479, -1, 481, 355, 357, -1, 482, 357, 359, -1, 482, 481, 357, -1, 402, 400, 483, -1, 402, 483, 355, -1, 402, 355, 481, -1, 478, 482, 359, -1, 479, 480, 368, -1, 484, 479, 368, -1, 371, 484, 368, -1, 287, 484, 371, -1, 485, 486, 302, -1, 486, 303, 302, -1, 487, 485, 291, -1, 485, 302, 291, -1, 488, 487, 290, -1, 487, 291, 290, -1, 489, 488, 293, -1, 488, 290, 293, -1, 490, 489, 294, -1, 489, 293, 294, -1, 491, 490, 295, -1, 490, 294, 295, -1, 492, 491, 306, -1, 491, 295, 306, -1, 493, 492, 307, -1, 492, 306, 307, -1, 494, 493, 308, -1, 493, 307, 308, -1, 495, 494, 288, -1, 494, 308, 288, -1, 496, 495, 300, -1, 495, 288, 300, -1, 496, 300, 299, -1, 497, 496, 299, -1, 497, 299, 297, -1, 498, 497, 297, -1, 498, 297, 296, -1, 402, 498, 296, -1, 498, 402, 499, -1, 500, 499, 481, -1, 500, 481, 501, -1, 501, 481, 482, -1, 501, 482, 502, -1, 502, 482, 478, -1, 502, 478, 503, -1, 503, 478, 477, -1, 503, 477, 504, -1, 504, 477, 479, -1, 504, 479, 505, -1, 505, 479, 484, -1, 505, 484, 506, -1, 287, 285, 507, -1, 506, 287, 507, -1, 507, 285, 304, -1, 507, 304, 508, -1, 508, 304, 305, -1, 508, 305, 486, -1, 486, 305, 303, -1, 499, 402, 481, -1, 506, 484, 287, -1, 509, 301, 310, -1, 510, 292, 301, -1, 510, 511, 292, -1, 510, 509, 511, -1, 510, 301, 509, -1, 512, 289, 511, -1, 511, 289, 292, -1, 309, 298, 513, -1, 298, 289, 514, -1, 289, 512, 514, -1, 512, 513, 514, -1, 513, 298, 514, -1, 509, 309, 513, -1, 310, 309, 509, -1, 395, 311, 392, -1, 392, 311, 312, -1, 515, 516, 500, -1, 358, 515, 500, -1, 516, 353, 499, -1, 500, 516, 499, -1, 360, 358, 501, -1, 358, 500, 501, -1, 350, 346, 498, -1, 353, 350, 498, -1, 499, 353, 498, -1, 362, 360, 502, -1, 363, 362, 502, -1, 360, 501, 502, -1, 346, 517, 497, -1, 498, 346, 497, -1, 365, 363, 503, -1, 363, 502, 503, -1, 342, 340, 496, -1, 517, 342, 496, -1, 497, 517, 496, -1, 367, 365, 504, -1, 365, 503, 504, -1, 340, 339, 495, -1, 496, 340, 495, -1, 367, 504, 505, -1, 369, 367, 505, -1, 369, 505, 370, -1, 495, 339, 494, -1, 370, 505, 506, -1, 494, 339, 337, -1, 370, 506, 372, -1, 372, 506, 507, -1, 493, 494, 518, -1, 494, 337, 518, -1, 372, 507, 374, -1, 492, 493, 519, -1, 493, 518, 519, -1, 507, 508, 375, -1, 374, 507, 375, -1, 492, 519, 332, -1, 508, 486, 377, -1, 375, 508, 377, -1, 491, 492, 330, -1, 492, 332, 330, -1, 377, 486, 316, -1, 490, 491, 520, -1, 491, 330, 520, -1, 486, 485, 521, -1, 316, 486, 521, -1, 489, 490, 326, -1, 490, 520, 326, -1, 485, 487, 522, -1, 521, 485, 522, -1, 489, 326, 325, -1, 522, 487, 320, -1, 488, 489, 323, -1, 489, 325, 323, -1, 487, 488, 322, -1, 488, 323, 322, -1, 320, 487, 322, -1, 382, 523, 524, -1, 523, 384, 524, -1, 384, 383, 524, -1, 383, 381, 524, -1, 381, 382, 524, -1, 525, 526, 387, -1, 387, 386, 527, -1, 386, 385, 527, -1, 385, 384, 527, -1, 384, 525, 527, -1, 525, 387, 527, -1, 387, 389, 388, -1, 391, 390, 389, -1, 395, 392, 394, -1, 394, 392, 528, -1, 399, 529, 382, -1, 382, 529, 530, -1, 531, 530, 391, -1, 530, 529, 391, -1, 528, 392, 391, -1, 529, 528, 391, -1, 399, 398, 532, -1, 398, 397, 532, -1, 397, 394, 532, -1, 394, 399, 532, -1, 533, 404, 534, -1, 535, 404, 403, -1, 534, 404, 535, -1, 536, 537, 406, -1, 536, 538, 537, -1, 404, 533, 536, -1, 404, 536, 406, -1, 458, 539, 540, -1, 541, 542, 540, -1, 543, 409, 406, -1, 543, 539, 409, -1, 543, 544, 545, -1, 543, 545, 546, -1, 547, 541, 540, -1, 543, 546, 548, -1, 543, 548, 549, -1, 543, 549, 550, -1, 543, 550, 551, -1, 543, 551, 552, -1, 553, 547, 540, -1, 543, 552, 539, -1, 543, 406, 544, -1, 554, 553, 540, -1, 552, 554, 540, -1, 406, 537, 555, -1, 406, 555, 556, -1, 406, 556, 557, -1, 406, 557, 558, -1, 406, 558, 544, -1, 539, 552, 540, -1, 559, 560, 408, -1, 559, 561, 560, -1, 559, 539, 561, -1, 559, 409, 539, -1, 559, 408, 409, -1, 408, 562, 563, -1, 564, 565, 415, -1, 456, 562, 458, -1, 566, 564, 415, -1, 567, 568, 456, -1, 569, 562, 456, -1, 569, 456, 568, -1, 570, 567, 456, -1, 571, 562, 569, -1, 572, 566, 415, -1, 573, 570, 456, -1, 574, 562, 571, -1, 575, 573, 456, -1, 410, 576, 577, -1, 578, 562, 574, -1, 410, 408, 576, -1, 414, 579, 580, -1, 563, 562, 578, -1, 414, 580, 572, -1, 414, 413, 410, -1, 414, 572, 415, -1, 581, 577, 582, -1, 581, 582, 583, -1, 581, 583, 584, -1, 581, 584, 585, -1, 581, 585, 579, -1, 581, 414, 410, -1, 581, 410, 577, -1, 581, 579, 414, -1, 415, 565, 586, -1, 415, 586, 587, -1, 415, 587, 575, -1, 415, 575, 456, -1, 408, 563, 576, -1, 588, 462, 589, -1, 589, 462, 590, -1, 589, 590, 591, -1, 591, 590, 464, -1, 591, 464, 592, -1, 592, 464, 466, -1, 592, 466, 593, -1, 593, 466, 467, -1, 593, 467, 594, -1, 594, 467, 595, -1, 594, 595, 596, -1, 596, 595, 469, -1, 596, 469, 597, -1, 597, 470, 598, -1, 598, 471, 599, -1, 599, 472, 600, -1, 600, 601, 602, -1, 602, 603, 604, -1, 604, 475, 605, -1, 475, 606, 605, -1, 605, 606, 607, -1, 605, 607, 608, -1, 608, 607, 609, -1, 610, 608, 611, -1, 608, 609, 611, -1, 608, 610, 612, -1, 610, 613, 612, -1, 613, 614, 612, -1, 614, 615, 612, -1, 615, 616, 612, -1, 617, 612, 618, -1, 612, 616, 618, -1, 619, 617, 620, -1, 617, 618, 620, -1, 621, 619, 622, -1, 619, 620, 622, -1, 621, 622, 623, -1, 623, 622, 624, -1, 623, 624, 625, -1, 625, 624, 626, -1, 625, 626, 627, -1, 627, 626, 628, -1, 627, 628, 629, -1, 629, 628, 630, -1, 629, 630, 631, -1, 631, 630, 632, -1, 631, 632, 633, -1, 633, 632, 634, -1, 633, 634, 635, -1, 635, 634, 636, -1, 635, 636, 637, -1, 637, 533, 534, -1, 637, 636, 536, -1, 533, 637, 536, -1, 637, 534, 638, -1, 534, 535, 638, -1, 535, 403, 638, -1, 403, 405, 639, -1, 638, 403, 639, -1, 638, 639, 640, -1, 638, 640, 411, -1, 411, 640, 407, -1, 411, 460, 588, -1, 460, 641, 588, -1, 638, 411, 588, -1, 588, 641, 462, -1, 597, 469, 470, -1, 598, 470, 471, -1, 599, 471, 472, -1, 600, 472, 601, -1, 602, 601, 603, -1, 604, 603, 475, -1, 640, 639, 405, -1, 407, 640, 405, -1, 415, 456, 457, -1, 415, 457, 416, -1, 457, 444, 442, -1, 457, 442, 440, -1, 457, 440, 438, -1, 457, 438, 436, -1, 434, 432, 416, -1, 436, 434, 416, -1, 457, 436, 416, -1, 416, 432, 430, -1, 416, 430, 428, -1, 416, 428, 417, -1, 642, 445, 446, -1, 643, 443, 445, -1, 643, 445, 642, -1, 644, 446, 447, -1, 644, 642, 446, -1, 645, 441, 443, -1, 645, 443, 643, -1, 646, 447, 448, -1, 646, 644, 447, -1, 647, 437, 439, -1, 647, 439, 441, -1, 647, 441, 645, -1, 648, 448, 449, -1, 648, 449, 450, -1, 648, 646, 448, -1, 649, 435, 437, -1, 649, 437, 647, -1, 650, 450, 451, -1, 650, 648, 450, -1, 651, 435, 649, -1, 652, 650, 451, -1, 433, 435, 651, -1, 452, 652, 451, -1, 653, 433, 651, -1, 654, 652, 452, -1, 431, 433, 653, -1, 453, 654, 452, -1, 655, 431, 653, -1, 656, 654, 453, -1, 429, 655, 657, -1, 429, 431, 655, -1, 454, 656, 453, -1, 454, 658, 656, -1, 427, 657, 659, -1, 427, 429, 657, -1, 455, 660, 658, -1, 455, 658, 454, -1, 426, 427, 659, -1, 419, 660, 455, -1, 425, 659, 661, -1, 425, 426, 659, -1, 420, 662, 660, -1, 420, 660, 419, -1, 424, 661, 663, -1, 424, 425, 661, -1, 421, 664, 662, -1, 421, 662, 420, -1, 423, 663, 665, -1, 423, 424, 663, -1, 422, 665, 664, -1, 422, 423, 665, -1, 422, 664, 421, -1, 611, 666, 610, -1, 611, 540, 666, -1, 606, 540, 611, -1, 561, 540, 606, -1, 667, 668, 561, -1, 667, 475, 668, -1, 667, 606, 475, -1, 667, 561, 606, -1, 512, 669, 670, -1, 671, 512, 670, -1, 530, 531, 672, -1, 531, 673, 672, -1, 673, 523, 672, -1, 523, 382, 672, -1, 382, 530, 672, -1, 526, 525, 674, -1, 525, 384, 674, -1, 384, 523, 674, -1, 523, 675, 674, -1, 675, 526, 674, -1, 531, 391, 387, -1, 391, 389, 387, -1, 526, 531, 387, -1, 529, 399, 676, -1, 399, 394, 676, -1, 394, 528, 676, -1, 528, 529, 676, -1, 634, 677, 538, -1, 634, 538, 636, -1, 636, 538, 536, -1, 678, 679, 680, -1, 679, 681, 680, -1, 626, 624, 680, -1, 628, 626, 680, -1, 682, 628, 680, -1, 681, 682, 680, -1, 624, 678, 680, -1, 622, 620, 683, -1, 624, 622, 684, -1, 622, 683, 684, -1, 685, 686, 687, -1, 620, 685, 687, -1, 683, 620, 687, -1, 624, 684, 678, -1, 632, 682, 681, -1, 632, 681, 677, -1, 632, 677, 634, -1, 558, 688, 689, -1, 558, 557, 688, -1, 544, 689, 690, -1, 544, 558, 689, -1, 545, 690, 691, -1, 545, 544, 690, -1, 546, 691, 692, -1, 546, 545, 691, -1, 548, 692, 693, -1, 548, 546, 692, -1, 549, 693, 694, -1, 549, 548, 693, -1, 550, 694, 695, -1, 550, 549, 694, -1, 551, 695, 696, -1, 551, 550, 695, -1, 552, 696, 697, -1, 552, 551, 696, -1, 554, 697, 698, -1, 554, 552, 697, -1, 553, 698, 699, -1, 553, 554, 698, -1, 547, 553, 699, -1, 547, 699, 700, -1, 541, 547, 700, -1, 541, 700, 701, -1, 542, 541, 701, -1, 542, 701, 687, -1, 702, 687, 701, -1, 683, 702, 703, -1, 704, 683, 703, -1, 684, 683, 704, -1, 705, 684, 704, -1, 678, 684, 705, -1, 706, 678, 705, -1, 679, 678, 706, -1, 707, 679, 706, -1, 681, 679, 707, -1, 708, 681, 707, -1, 677, 681, 708, -1, 709, 677, 708, -1, 710, 537, 538, -1, 710, 538, 709, -1, 555, 537, 710, -1, 711, 555, 710, -1, 556, 555, 711, -1, 688, 556, 711, -1, 557, 556, 688, -1, 683, 687, 702, -1, 538, 677, 709, -1, 666, 540, 686, -1, 712, 666, 686, -1, 542, 686, 540, -1, 687, 686, 542, -1, 587, 653, 651, -1, 577, 654, 656, -1, 587, 586, 653, -1, 577, 576, 654, -1, 649, 587, 651, -1, 582, 656, 658, -1, 575, 587, 649, -1, 584, 583, 660, -1, 582, 577, 656, -1, 584, 660, 662, -1, 660, 582, 658, -1, 573, 575, 649, -1, 664, 584, 662, -1, 573, 649, 647, -1, 583, 582, 660, -1, 585, 584, 664, -1, 570, 573, 647, -1, 570, 647, 645, -1, 579, 664, 665, -1, 579, 585, 664, -1, 567, 570, 645, -1, 567, 645, 643, -1, 580, 665, 663, -1, 580, 579, 665, -1, 568, 567, 643, -1, 568, 643, 642, -1, 572, 663, 661, -1, 569, 568, 642, -1, 572, 580, 663, -1, 569, 642, 644, -1, 646, 569, 644, -1, 566, 661, 659, -1, 566, 572, 661, -1, 571, 569, 646, -1, 648, 571, 646, -1, 564, 659, 657, -1, 574, 571, 648, -1, 564, 566, 659, -1, 650, 574, 648, -1, 578, 574, 650, -1, 565, 657, 655, -1, 652, 578, 650, -1, 565, 564, 657, -1, 563, 578, 652, -1, 653, 565, 655, -1, 586, 565, 653, -1, 576, 652, 654, -1, 576, 563, 652, -1, 703, 713, 621, -1, 702, 617, 713, -1, 702, 713, 703, -1, 704, 621, 623, -1, 704, 703, 621, -1, 701, 612, 617, -1, 701, 617, 702, -1, 705, 623, 625, -1, 705, 704, 623, -1, 700, 605, 608, -1, 700, 608, 612, -1, 700, 612, 701, -1, 706, 625, 627, -1, 706, 627, 629, -1, 706, 705, 625, -1, 699, 604, 605, -1, 699, 605, 700, -1, 707, 629, 631, -1, 707, 706, 629, -1, 698, 604, 699, -1, 708, 707, 631, -1, 602, 604, 698, -1, 633, 708, 631, -1, 697, 602, 698, -1, 709, 708, 633, -1, 600, 602, 697, -1, 714, 709, 633, -1, 696, 600, 697, -1, 710, 709, 714, -1, 599, 696, 695, -1, 599, 600, 696, -1, 637, 710, 714, -1, 637, 711, 710, -1, 598, 695, 694, -1, 598, 599, 695, -1, 638, 688, 711, -1, 638, 711, 637, -1, 597, 598, 694, -1, 588, 688, 638, -1, 596, 694, 693, -1, 596, 597, 694, -1, 589, 689, 688, -1, 589, 688, 588, -1, 715, 693, 692, -1, 715, 596, 693, -1, 591, 690, 689, -1, 591, 689, 589, -1, 593, 692, 691, -1, 593, 715, 692, -1, 592, 691, 690, -1, 592, 593, 691, -1, 592, 690, 591, -1, 607, 606, 611, -1, 609, 607, 611, -1, 666, 615, 614, -1, 666, 614, 613, -1, 666, 613, 610, -1, 523, 526, 675, -1, 531, 526, 716, -1, 523, 673, 716, -1, 673, 531, 716, -1, 526, 523, 716, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 5.793 -4.050 69.673 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 5.793 -4.050 -119.735 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 5.793 -98.754 -25.031 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 5.793 90.654 -25.031 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -88.911 -4.050 -25.031 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 100.497 -4.050 -25.031 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_SHOULDER_P.wrl000066400000000000000000002226531207742442300235620ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 36.000 32.467 33.997 center 0.000 0.000 0.000 #translation 2.000 17.234 0.001 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.80 0.80 0.80 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 2.828 -3.000 -2.828, 3.464 -3.000 -2.000, 3.464 -1.000 -2.000, -2.000 -3.000 3.464, -2.828 -1.000 2.828, -2.000 -1.000 3.464, -2.828 -3.000 2.828, 2.000 -3.000 -3.464, 2.828 -1.000 -2.828, 2.000 -1.000 -3.464, -1.035 -1.000 -3.864, 0.000 -3.000 -4.000, 0.000 -1.000 -4.000, -1.035 -3.000 3.864, -1.035 -1.000 3.864, -1.035 -3.000 -3.864, 1.035 -3.000 -3.864, 1.035 -1.000 -3.864, 0.000 -1.000 4.000, -2.000 -3.000 -3.464, -2.000 -1.000 -3.464, 0.000 -3.000 4.000, 1.035 -1.000 3.864, 1.035 -3.000 3.864, -2.828 -3.000 -2.828, -2.828 -1.000 -2.828, 2.000 -3.000 3.464, 2.000 -1.000 3.464, -3.464 -3.000 -2.000, -3.464 -1.000 -2.000, 2.828 -3.000 2.828, 2.828 -1.000 2.828, -3.864 -3.000 -1.035, -3.864 -1.000 -1.035, 3.464 -3.000 2.000, 3.464 -1.000 2.000, -4.000 -3.000 -0.000, -4.000 -1.000 -0.000, 3.864 -3.000 1.035, 3.864 -1.000 1.035, -3.864 -3.000 1.035, -3.864 -1.000 1.035, 4.000 -3.000 -0.000, 4.000 -1.000 -0.000, 3.864 -3.000 -1.035, 3.864 -1.000 -1.035, -3.464 -3.000 2.000, -3.464 -1.000 2.000, 12.000 -1.000 11.000, -16.000 -1.000 3.500, -16.000 -1.000 11.000, -16.000 -1.000 -13.500, 12.000 -1.000 -13.500, 12.000 -3.000 11.000, -16.000 -3.000 11.000, -16.000 -3.000 3.500, -16.000 -3.000 -13.500, 12.000 -3.000 -13.500, -18.000 -1.000 3.500, -18.000 -1.000 11.000, -18.000 -1.000 -13.500, 14.000 -1.000 11.000, 14.000 -1.000 -13.500, -16.000 -18.292 -11.971, -16.000 -3.000 -16.500, -16.000 -20.420 3.500, -16.000 -19.298 -3.446, -16.000 -25.500 -16.500, -16.000 -23.720 -16.299, -16.000 -29.197 -1.405, -16.000 -27.498 -0.754, -16.000 -16.099 -6.500, -16.000 -17.500 -8.500, -16.000 -17.701 -10.280, -16.000 -17.707 -6.692, -16.000 -18.317 -4.978, -16.000 -22.029 -15.708, -16.000 -20.512 -14.755, -16.000 -19.245 -13.488, -16.000 -20.600 -2.176, -16.000 -22.156 -1.232, -16.000 -23.884 -0.665, -16.000 -25.697 -0.502, 12.000 -22.156 -1.232, 12.000 -23.884 -0.665, 12.000 -7.000 11.000, 12.000 -25.697 -0.502, 12.000 -23.720 -16.299, 12.000 -3.000 -16.500, 12.000 -25.500 -16.500, 12.000 -27.498 -0.754, 12.000 -29.197 -1.405, 12.000 -16.099 -2.750, 12.000 -17.500 -8.500, 12.000 -17.707 -6.692, 12.000 -18.317 -4.978, 12.000 -19.298 -3.446, 12.000 -20.600 -2.176, 12.000 -22.029 -15.708, 12.000 -20.512 -14.755, 12.000 -19.245 -13.488, 12.000 -18.292 -11.971, 12.000 -17.701 -10.280, -18.000 -3.000 11.000, -18.000 -3.000 3.500, -18.000 -3.000 -13.500, -16.000 -1.000 -16.500, -18.000 -1.000 -16.500, 14.000 -3.000 11.000, 12.000 -1.000 -16.500, 14.000 -3.000 -13.500, 14.000 -1.000 -16.500, -18.000 -20.420 3.500, -18.000 -3.000 -16.500, -18.000 -25.500 -16.500, -16.000 -27.498 -0.754, -16.000 -27.318 -3.842, -16.000 -25.972 -3.522, -16.000 -29.197 -1.405, -16.000 -17.701 -10.280, -16.000 -20.500 -8.500, -16.000 -20.693 -9.875, -16.000 -18.292 -11.971, -16.000 -26.000 -13.475, -16.000 -26.000 -16.484, -16.000 -25.833 -16.493, -16.000 -25.667 -16.498, -16.000 -25.500 -16.500, -16.000 -24.613 -13.421, -16.000 -23.720 -16.299, -16.000 -23.295 -12.987, -16.000 -22.029 -15.708, -16.000 -25.697 -0.502, -16.000 -24.590 -3.583, -16.000 -20.512 -14.755, -16.000 -17.500 -8.500, -16.000 -23.884 -0.665, -16.000 -22.146 -12.208, -16.000 -17.707 -6.692, -16.000 -20.691 -7.130, -16.000 -19.245 -13.488, -16.000 -22.156 -1.232, -16.000 -23.278 -4.021, -16.000 -21.256 -11.143, -16.000 -18.317 -4.978, -16.000 -21.251 -5.865, -16.000 -29.500 -1.575, -16.000 -28.525 -4.519, -16.000 -29.500 -5.500, -16.000 -20.600 -2.176, -16.000 -22.136 -4.801, -16.000 -19.298 -3.446, -18.000 -29.197 -1.405, -16.000 -29.197 -1.405, -16.000 -20.420 3.500, -18.000 -20.420 3.500, 14.000 -7.000 11.000, 14.000 -3.000 -16.500, 14.000 -25.500 -16.500, 12.000 -22.122 -10.643, 12.000 -21.659 -9.616, 12.000 -18.292 -11.971, 12.000 -19.245 -13.488, 12.000 -25.833 -16.493, 12.000 -26.000 -16.484, 12.000 -26.000 -12.469, 12.000 -25.667 -16.498, 12.000 -25.500 -16.500, 12.000 -23.720 -16.299, 12.000 -24.873 -12.451, 12.000 -25.794 -4.511, 12.000 -26.862 -4.739, 12.000 -27.498 -0.754, 12.000 -29.197 -1.405, 12.000 -21.500 -8.500, 12.000 -17.701 -10.280, 12.000 -22.029 -15.708, 12.000 -23.796 -12.119, 12.000 -24.703 -4.580, 12.000 -25.697 -0.502, 12.000 -20.512 -14.755, 12.000 -17.500 -8.500, 12.000 -22.854 -11.500, 12.000 -28.623 -6.000, 12.000 -29.500 -6.000, 12.000 -27.829 -5.248, 12.000 -23.884 -0.665, 12.000 -21.649 -7.417, 12.000 -17.707 -6.692, 12.000 -22.156 -1.232, 12.000 -23.672 -4.942, 12.000 -22.086 -6.416, 12.000 -18.317 -4.978, 12.000 -29.500 -1.575, 12.000 -20.600 -2.176, 12.000 -22.777 -5.570, 12.000 -19.298 -3.446, 14.000 -29.197 -1.405, 14.000 -7.000 11.000, 12.000 -7.000 11.000, 12.000 -29.197 -1.405, -18.000 -18.292 -11.971, -18.000 -17.701 -10.280, -18.000 -19.298 -3.446, -18.000 -23.720 -16.299, -18.000 -27.498 -0.754, -18.000 -29.197 -1.405, -18.000 -17.500 -8.500, -18.000 -16.099 -6.500, -18.000 -17.707 -6.692, -18.000 -18.317 -4.978, -18.000 -22.029 -15.708, -18.000 -20.512 -14.755, -18.000 -19.245 -13.488, -18.000 -20.600 -2.176, -18.000 -22.156 -1.232, -18.000 -23.884 -0.665, -18.000 -25.697 -0.502, 14.000 -23.884 -0.665, 14.000 -22.156 -1.232, 14.000 -25.697 -0.502, 14.000 -23.720 -16.299, 14.000 -27.498 -0.754, 14.000 -29.197 -1.405, 14.000 -16.099 -2.750, 14.000 -17.707 -6.692, 14.000 -17.500 -8.500, 14.000 -18.317 -4.978, 14.000 -19.298 -3.446, 14.000 -20.600 -2.176, 14.000 -22.029 -15.708, 14.000 -20.512 -14.755, 14.000 -19.245 -13.488, 14.000 -18.292 -11.971, 14.000 -17.701 -10.280, -18.000 -25.500 -16.500, -16.000 -25.500 -16.500, -16.000 -26.000 -16.500, -18.000 -26.000 -16.500, -16.000 -26.000 -16.500, -16.000 -30.145 -6.649, -16.000 -32.924 -5.520, -16.000 -33.416 -7.341, -16.000 -30.467 -7.926, -16.000 -32.021 -3.865, -16.000 -30.755 -2.468, -16.000 -29.395 -11.635, -16.000 -32.264 -12.772, -16.000 -31.076 -14.236, -16.000 -28.440 -12.544, -16.000 -29.579 -15.382, -16.000 -30.078 -10.509, -16.000 -33.076 -11.070, -16.000 -27.282 -13.172, -16.000 -27.855 -16.146, -16.000 -30.444 -9.244, -16.000 -33.467 -9.225, -16.000 -30.145 -6.649, -18.000 -29.500 -5.500, -16.000 -29.500 -5.500, -18.000 -20.693 -9.875, -16.000 -21.256 -11.143, -16.000 -20.693 -9.875, -18.000 -21.256 -11.143, -18.000 -30.145 -6.649, -16.000 -20.500 -8.500, -16.000 -30.467 -7.926, -18.000 -20.500 -8.500, -18.000 -20.691 -7.130, -16.000 -20.691 -7.130, -18.000 -30.467 -7.926, -16.000 -30.444 -9.244, -18.000 -30.444 -9.244, -16.000 -30.078 -10.509, -18.000 -21.251 -5.865, -16.000 -21.251 -5.865, -18.000 -30.078 -10.509, -16.000 -29.395 -11.635, -18.000 -22.136 -4.801, -16.000 -22.136 -4.801, -18.000 -29.395 -11.635, -16.000 -28.440 -12.544, -18.000 -23.278 -4.021, -16.000 -23.278 -4.021, -18.000 -28.440 -12.544, -16.000 -27.282 -13.172, -18.000 -27.282 -13.172, -18.000 -24.590 -3.583, -16.000 -24.590 -3.583, -16.000 -26.000 -13.475, -18.000 -26.000 -13.475, -18.000 -25.972 -3.522, -16.000 -25.972 -3.522, -18.000 -24.613 -13.421, -16.000 -24.613 -13.421, -18.000 -27.318 -3.842, -16.000 -27.318 -3.842, -18.000 -23.295 -12.987, -16.000 -23.295 -12.987, -18.000 -28.525 -4.519, -16.000 -28.525 -4.519, -18.000 -22.146 -12.208, -16.000 -22.146 -12.208, -16.000 -26.000 -16.484, -16.000 -27.855 -16.146, -18.000 -26.000 -16.484, -18.000 -27.855 -16.146, -16.000 -30.755 -2.468, -18.000 -30.755 -2.468, -16.000 -32.021 -3.865, -18.000 -32.021 -3.865, -16.000 -32.924 -5.520, -18.000 -32.924 -5.520, -16.000 -33.416 -7.341, -18.000 -33.416 -7.341, -16.000 -33.467 -9.225, -18.000 -33.467 -9.225, -16.000 -33.076 -11.070, -18.000 -33.076 -11.070, -16.000 -32.264 -12.772, -18.000 -32.264 -12.772, -16.000 -31.076 -14.236, -18.000 -31.076 -14.236, -16.000 -29.579 -15.382, -18.000 -29.579 -15.382, 12.000 -25.500 -16.500, 14.000 -25.500 -16.500, 14.000 -26.000 -16.500, 12.000 -26.000 -16.500, 12.000 -26.000 -16.500, 12.000 -29.168 -6.905, 12.000 -29.458 -7.922, 12.000 -32.924 -5.520, 12.000 -32.021 -3.865, 12.000 -33.416 -7.341, 12.000 -30.755 -2.468, 12.000 -29.471 -8.979, 12.000 -29.734 -8.945, 12.000 -29.207 -10.003, 12.000 -33.467 -9.225, 12.000 -33.076 -11.070, 12.000 -29.579 -15.382, 12.000 -31.076 -14.236, 12.000 -27.938 -11.671, 12.000 -32.264 -12.772, 12.000 -28.684 -10.921, 12.000 -27.855 -16.146, 12.000 -27.022 -12.199, 12.000 -22.122 -10.643, 12.000 -22.854 -11.500, 14.000 -22.122 -10.643, 12.000 -29.168 -6.905, 14.000 -28.623 -6.000, 14.000 -29.168 -6.905, 12.000 -28.623 -6.000, 14.000 -21.659 -9.616, 12.000 -21.659 -9.616, 12.000 -29.458 -7.922, 14.000 -29.458 -7.922, 12.000 -21.649 -7.417, 14.000 -21.500 -8.500, 14.000 -21.649 -7.417, 12.000 -21.500 -8.500, 12.000 -29.471 -8.979, 14.000 -29.471 -8.979, 12.000 -22.086 -6.416, 14.000 -22.086 -6.416, 12.000 -29.207 -10.003, 12.000 -22.777 -5.570, 14.000 -22.777 -5.570, 14.000 -29.207 -10.003, 12.000 -28.684 -10.921, 14.000 -28.684 -10.921, 12.000 -23.672 -4.942, 14.000 -23.672 -4.942, 12.000 -27.938 -11.671, 14.000 -27.938 -11.671, 12.000 -24.703 -4.580, 14.000 -24.703 -4.580, 12.000 -27.022 -12.199, 14.000 -27.022 -12.199, 12.000 -25.794 -4.511, 14.000 -25.794 -4.511, 12.000 -26.000 -12.469, 14.000 -26.000 -12.469, 12.000 -26.862 -4.739, 14.000 -26.862 -4.739, 12.000 -24.873 -12.451, 14.000 -24.873 -12.451, 14.000 -23.796 -12.119, 12.000 -27.829 -5.248, 14.000 -27.829 -5.248, 12.000 -23.796 -12.119, 14.000 -22.854 -11.500, 14.000 -26.000 -16.484, 14.000 -27.855 -16.146, 12.000 -26.000 -16.484, 12.000 -27.855 -16.146, 14.000 -30.755 -2.468, 12.000 -30.755 -2.468, 14.000 -32.021 -3.865, 12.000 -32.021 -3.865, 14.000 -32.924 -5.520, 12.000 -32.924 -5.520, 14.000 -33.416 -7.341, 12.000 -33.416 -7.341, 14.000 -33.467 -9.225, 12.000 -33.467 -9.225, 14.000 -33.076 -11.070, 12.000 -33.076 -11.070, 14.000 -32.264 -12.772, 12.000 -32.264 -12.772, 14.000 -31.076 -14.236, 12.000 -31.076 -14.236, 14.000 -29.579 -15.382, 12.000 -29.579 -15.382, -18.000 -27.498 -0.754, -18.000 -17.701 -10.280, -18.000 -18.292 -11.971, -18.000 -25.833 -16.493, -18.000 -25.667 -16.498, -18.000 -23.720 -16.299, -18.000 -22.029 -15.708, -18.000 -25.697 -0.502, -18.000 -20.512 -14.755, -18.000 -17.500 -8.500, -18.000 -23.884 -0.665, -18.000 -17.707 -6.692, -18.000 -19.245 -13.488, -18.000 -22.156 -1.232, -18.000 -18.317 -4.978, -18.000 -29.500 -1.575, -18.000 -20.600 -2.176, -18.000 -19.298 -3.446, 14.000 -18.292 -11.971, 14.000 -19.245 -13.488, 14.000 -25.833 -16.493, 14.000 -25.667 -16.498, 14.000 -23.720 -16.299, 14.000 -27.498 -0.754, 14.000 -22.029 -15.708, 14.000 -17.701 -10.280, 14.000 -25.697 -0.502, 14.000 -20.512 -14.755, 14.000 -17.500 -8.500, 14.000 -29.500 -6.000, 14.000 -23.884 -0.665, 14.000 -17.707 -6.692, 14.000 -22.156 -1.232, 14.000 -18.317 -4.978, 14.000 -29.500 -1.575, 14.000 -20.600 -2.176, 14.000 -19.298 -3.446, 14.000 -29.734 -8.945, 15.000 -29.485 1.699, 15.000 -29.456 2.510, 15.000 -28.161 1.492, 15.000 -29.295 0.909, 15.000 -28.900 0.200, 15.000 -28.329 -0.378, 15.000 -27.624 -0.781, 15.000 -26.837 -0.981, 15.000 -29.211 3.284, 15.000 -28.767 3.964, 15.000 -28.940 3.742, 15.000 -28.883 3.816, 15.000 -26.220 2.008, 15.000 -23.608 2.799, 15.000 -23.500 2.000, 15.000 -23.926 3.541, 15.000 -24.430 4.171, 15.000 -25.083 4.644, 15.000 -25.839 4.926, 15.000 -26.643 4.997, 15.000 -27.436 4.850, 15.000 -28.162 4.498, 15.000 -25.998 -0.958, 15.000 -25.199 -0.703, 15.000 -24.501 -0.237, 15.000 -23.960 0.404, 15.000 -23.617 1.169, 15.000 -28.825 3.891, 16.000 -29.211 3.284, 15.000 -29.211 3.284, 16.000 -28.767 3.964, 16.000 -29.456 2.510, 15.000 -29.456 2.510, 16.000 -29.485 1.699, 15.000 -29.485 1.699, 16.000 -29.295 0.909, 15.000 -29.295 0.909, 16.000 -28.900 0.200, 15.000 -28.900 0.200, 16.000 -28.329 -0.378, 15.000 -28.329 -0.378, 16.000 -27.624 -0.781, 15.000 -27.624 -0.781, 16.000 -26.837 -0.981, 15.000 -18.876 -17.000, 15.000 -18.878 -16.997, 15.000 -18.255 -16.886, 15.000 -21.191 -13.295, 15.000 -21.221 -13.247, 15.000 -20.801 -12.766, 15.000 -20.550 -11.916, 15.000 -20.286 -12.356, 15.000 -19.682 -12.095, 15.000 -19.031 -12.000, 15.000 -20.139 -10.484, 15.000 -18.378 -12.079, 15.000 -17.768 -12.325, 15.000 -20.000 -9.000, 15.000 -17.243 -12.721, 15.000 -20.223 -7.126, 15.000 -26.787 -1.092, 15.000 -24.969 -1.596, 15.000 -23.319 -2.512, 15.000 -21.930 -3.789, 15.000 -17.680 -16.623, 15.000 -17.189 -16.224, 15.000 -5.406 -16.422, 15.000 -16.815 -15.714, 15.000 -5.724 -16.013, 15.000 -4.993 -16.736, 15.000 -16.580 -15.127, 15.000 -5.927 -15.536, 15.000 -4.514 -16.933, 15.000 -16.500 -14.500, 15.000 -6.000 -15.023, 15.000 -4.000 -17.000, 15.000 -16.587 -13.848, 15.000 -5.939 -14.509, 15.000 -16.840 -13.241, 15.000 -5.747 -14.027, 15.000 -26.104 6.979, 15.000 -5.439 -13.611, 15.000 -2.000 -14.929, 15.000 -2.001 -14.930, 15.000 -2.086 -14.419, 15.000 -23.103 9.658, 15.000 -19.807 11.964, 15.000 -5.444 13.616, 15.000 -12.516 15.333, 15.000 -5.743 14.018, 15.000 -8.624 16.349, 15.000 -5.932 14.483, 15.000 -6.000 14.980, 15.000 -5.942 15.478, 15.000 -3.531 -13.056, 15.000 -3.604 13.040, 15.000 -3.129 13.200, 15.000 -3.047 -13.241, 15.000 -5.762 15.946, 15.000 -2.708 13.473, 15.000 -2.628 -13.545, 15.000 -5.471 16.355, 15.000 -2.369 13.842, 15.000 -2.300 -13.947, 15.000 -5.088 16.678, 15.000 -2.132 14.284, 15.000 -4.638 16.897, 15.000 -4.637 16.896, 15.000 -2.013 14.771, 15.000 -2.000 14.761, 15.000 -20.878 -5.356, 15.000 -15.384 -0.051, 15.000 -16.261 13.865, 15.000 -5.054 13.300, 15.000 -4.598 13.091, 15.000 -4.104 13.003, 15.000 -4.047 -13.001, 15.000 -4.559 -13.080, 15.000 -5.033 -13.288, 16.000 -26.492 1.998, 16.000 -23.500 2.000, 16.000 -23.608 2.799, 16.000 -23.926 3.541, 16.000 -24.430 4.171, 16.000 -25.083 4.644, 16.000 -25.839 4.926, 16.000 -26.643 4.997, 16.000 -27.436 4.850, 16.000 -28.162 4.498, 16.000 -26.500 -1.000, 16.000 -25.724 -0.898, 16.000 -25.000 -0.598, 16.000 -24.379 -0.121, 16.000 -23.902 0.500, 16.000 -23.602 1.224, 16.000 -26.725 -0.992, 16.000 -26.613 -0.998, 16.000 -26.787 -1.092, 15.000 -26.787 -1.092, 16.000 -8.624 16.349, 16.000 -4.638 16.897, 15.000 -4.638 16.897, 16.000 -12.516 15.333, 15.000 -12.516 15.333, 16.000 -16.261 13.865, 16.000 -19.807 11.964, 15.000 -19.807 11.964, 16.000 -23.103 9.658, 15.000 -23.103 9.658, 16.000 -26.104 6.979, 15.000 -26.104 6.979, 16.000 -18.878 -16.997, 15.000 -18.878 -16.997, 15.000 -18.876 -17.000, 16.000 -18.876 -17.000, 15.000 -18.845 -14.499, 16.000 -21.221 -13.247, 15.000 -21.191 -13.295, 16.000 -21.191 -13.295, 16.000 -20.550 -11.916, 15.000 -20.139 -10.484, 16.000 -20.139 -10.484, 15.000 -20.000 -9.000, 16.000 -20.000 -9.000, 15.000 -20.223 -7.126, 16.000 -20.223 -7.126, 15.000 -20.878 -5.356, 16.000 -20.878 -5.356, 16.000 -21.930 -3.789, 16.000 -23.319 -2.512, 16.000 -24.969 -1.596, 15.000 -4.637 16.896, 16.000 -4.637 16.896, 15.000 -4.006 14.949, 16.000 -2.013 14.771, 15.000 -2.013 14.771, 16.000 -2.000 14.761, 16.000 -2.000 -14.929, 16.000 -2.001 -14.930, 15.000 -4.001 -15.000, 16.000 -4.000 -17.000, 16.000 -18.255 -16.886, 16.000 -20.801 -12.766, 16.000 -20.286 -12.356, 16.000 -19.682 -12.095, 16.000 -19.031 -12.000, 16.000 -18.378 -12.079, 16.000 -17.768 -12.325, 16.000 -17.243 -12.721, 16.000 -5.406 -16.422, 16.000 -17.189 -16.224, 16.000 -17.680 -16.623, 16.000 -5.724 -16.013, 16.000 -16.815 -15.714, 16.000 -4.993 -16.736, 16.000 -5.927 -15.536, 16.000 -16.580 -15.127, 16.000 -4.514 -16.933, 16.000 -6.000 -15.023, 16.000 -16.500 -14.500, 16.000 -5.939 -14.509, 16.000 -16.587 -13.848, 16.000 -5.747 -14.027, 16.000 -16.840 -13.241, 16.000 -5.439 -13.611, 16.000 -2.086 -14.419, 16.000 -5.709 13.961, 16.000 -5.390 13.562, 16.000 -5.916 14.428, 16.000 -5.999 14.932, 16.000 -5.951 15.441, 16.000 -3.426 13.084, 16.000 -3.531 -13.056, 16.000 -3.047 -13.241, 16.000 -4.000 13.000, 16.000 -5.775 15.921, 16.000 -2.900 13.330, 16.000 -2.628 -13.545, 16.000 -5.484 16.340, 16.000 -2.466 13.716, 16.000 -2.300 -13.947, 16.000 -5.096 16.673, 16.000 -2.162 14.211, 16.000 -15.384 -0.051, 16.000 -4.047 -13.001, 16.000 -4.559 -13.080, 16.000 -5.033 -13.288, 16.000 -4.980 13.257, 16.000 -4.507 13.065, 16.000 -21.426 -13.897, 15.000 -21.430 -13.912, 16.000 -21.500 -14.539, 15.000 -21.499 -14.570, 16.000 -21.406 -15.178, 15.000 -21.393 -15.222, 16.000 -21.152 -15.772, 15.000 -21.120 -15.825, 16.000 -20.754 -16.281, 15.000 -20.699 -16.334, 16.000 -20.239 -16.672, 15.000 -20.158 -16.716, 16.000 -19.641 -16.916, 15.000 -19.537 -16.942, 16.000 -19.000 -17.000, 16.000 -18.959 -17.000, 16.000 -18.918 -16.999, 15.000 -20.158 -16.716, 15.000 -19.537 -16.942, 15.000 -21.499 -14.570, 15.000 -21.430 -13.912, 15.000 -21.393 -15.222, 15.000 -2.073 15.535, 16.000 -2.000 15.000, 15.000 -2.000 15.000, 16.000 -2.073 15.535, 15.000 -2.286 16.031, 16.000 -2.286 16.031, 15.000 -2.624 16.451, 16.000 -2.624 16.451, 15.000 -3.062 16.766, 16.000 -3.062 16.766, 15.000 -3.569 16.953, 16.000 -3.569 16.953, 15.000 -4.107 16.997, 16.000 -4.107 16.997, 15.000 -2.006 14.847, 15.000 -3.318 15.884, 15.000 -2.001 14.924, 15.000 -2.286 16.031, 15.000 -2.624 16.451, 15.000 -3.062 16.766, 15.000 -2.006 14.847, 16.000 -2.006 14.847, 15.000 -2.001 14.924, 16.000 -2.001 14.924, 15.000 -2.000 -14.977, 16.000 -2.000 -15.000, 15.000 -2.000 -15.000, 16.000 -2.000 -14.977, 15.000 -2.001 -14.953, 16.000 -2.001 -14.953, 15.000 -3.482 -16.932, 15.000 -3.000 -16.732, 15.000 -2.586 -16.414, 15.000 -2.268 -16.000, 15.000 -2.068 -15.518, 16.000 -3.482 -16.932, 16.000 -3.000 -16.732, 15.000 -3.000 -16.732, 16.000 -2.586 -16.414, 15.000 -2.586 -16.414, 16.000 -2.268 -16.000, 16.000 -2.068 -15.518, 16.000 -4.000 -15.000, 16.000 -19.000 -14.500, 16.000 -3.999 14.999, -20.000 -29.485 1.699, -20.000 -29.456 2.510, -20.000 -28.161 1.492, -20.000 -29.295 0.909, -20.000 -28.900 0.200, -20.000 -28.329 -0.378, -20.000 -27.624 -0.781, -20.000 -26.837 -0.981, -20.000 -29.211 3.284, -20.000 -28.767 3.964, -20.000 -28.940 3.742, -20.000 -28.883 3.816, -20.000 -26.220 2.008, -20.000 -23.608 2.799, -20.000 -23.500 2.000, -20.000 -23.926 3.541, -20.000 -24.430 4.171, -20.000 -25.083 4.644, -20.000 -25.839 4.926, -20.000 -26.643 4.997, -20.000 -27.436 4.850, -20.000 -28.162 4.498, -20.000 -25.998 -0.958, -20.000 -25.199 -0.703, -20.000 -24.501 -0.237, -20.000 -23.960 0.404, -20.000 -23.617 1.169, -20.000 -28.825 3.891, -19.000 -29.211 3.284, -19.000 -28.767 3.964, -19.000 -29.456 2.510, -20.000 -29.456 2.510, -19.000 -29.485 1.699, -19.000 -29.295 0.909, -19.000 -28.900 0.200, -19.000 -28.329 -0.378, -20.000 -28.329 -0.378, -19.000 -27.624 -0.781, -19.000 -26.837 -0.981, -20.000 -18.876 -17.000, -20.000 -18.878 -16.997, -20.000 -18.255 -16.886, -20.000 -21.191 -13.295, -20.000 -21.221 -13.247, -20.000 -20.801 -12.766, -20.000 -20.550 -11.916, -20.000 -20.286 -12.356, -20.000 -19.682 -12.095, -20.000 -19.031 -12.000, -20.000 -20.139 -10.484, -20.000 -18.378 -12.079, -20.000 -17.768 -12.325, -20.000 -20.000 -9.000, -20.000 -17.243 -12.721, -20.000 -20.223 -7.126, -20.000 -26.787 -1.092, -20.000 -24.969 -1.596, -20.000 -23.319 -2.512, -20.000 -21.930 -3.789, -20.000 -17.680 -16.623, -20.000 -17.189 -16.224, -20.000 -5.406 -16.422, -20.000 -16.815 -15.714, -20.000 -5.724 -16.013, -20.000 -4.993 -16.736, -20.000 -16.580 -15.127, -20.000 -5.927 -15.536, -20.000 -4.514 -16.933, -20.000 -16.500 -14.500, -20.000 -6.000 -15.023, -20.000 -4.000 -17.000, -20.000 -16.587 -13.848, -20.000 -5.939 -14.509, -20.000 -16.840 -13.241, -20.000 -5.747 -14.027, -20.000 -26.104 6.979, -20.000 -5.439 -13.611, -20.000 -2.000 -14.929, -20.000 -2.001 -14.930, -20.000 -2.086 -14.419, -20.000 -23.103 9.658, -20.000 -19.807 11.964, -20.000 -5.444 13.616, -20.000 -12.516 15.333, -20.000 -5.743 14.018, -20.000 -8.624 16.349, -20.000 -5.932 14.483, -20.000 -6.000 14.980, -20.000 -5.942 15.478, -20.000 -3.531 -13.056, -20.000 -3.604 13.040, -20.000 -3.129 13.200, -20.000 -3.047 -13.241, -20.000 -5.762 15.946, -20.000 -2.708 13.473, -20.000 -2.628 -13.545, -20.000 -5.471 16.355, -20.000 -2.369 13.842, -20.000 -2.300 -13.947, -20.000 -5.088 16.678, -20.000 -2.132 14.284, -20.000 -4.638 16.897, -20.000 -4.637 16.896, -20.000 -2.013 14.771, -20.000 -2.000 14.761, -20.000 -15.384 -0.051, -20.000 -20.878 -5.356, -20.000 -16.261 13.865, -20.000 -5.054 13.300, -20.000 -4.598 13.091, -20.000 -4.104 13.003, -20.000 -4.047 -13.001, -20.000 -4.559 -13.080, -20.000 -5.033 -13.288, -19.000 -26.492 1.998, -19.000 -23.500 2.000, -19.000 -23.608 2.799, -19.000 -23.926 3.541, -19.000 -24.430 4.171, -19.000 -25.083 4.644, -19.000 -25.839 4.926, -19.000 -26.643 4.997, -19.000 -27.436 4.850, -19.000 -28.162 4.498, -19.000 -26.500 -1.000, -19.000 -25.724 -0.898, -19.000 -25.000 -0.598, -19.000 -24.379 -0.121, -19.000 -23.902 0.500, -19.000 -23.602 1.224, -19.000 -26.725 -0.992, -19.000 -26.613 -0.998, -19.000 -26.787 -1.092, -19.000 -8.624 16.349, -19.000 -4.638 16.897, -19.000 -12.516 15.333, -20.000 -12.516 15.333, -19.000 -16.261 13.865, -19.000 -19.807 11.964, -19.000 -23.103 9.658, -19.000 -26.104 6.979, -19.000 -18.878 -16.997, -20.000 -18.878 -16.997, -19.000 -18.876 -17.000, -20.000 -18.845 -14.499, -19.000 -21.221 -13.247, -19.000 -21.191 -13.295, -19.000 -20.550 -11.916, -20.000 -20.139 -10.484, -19.000 -20.139 -10.484, -20.000 -20.000 -9.000, -19.000 -20.000 -9.000, -19.000 -20.223 -7.126, -19.000 -20.878 -5.356, -19.000 -21.930 -3.789, -19.000 -23.319 -2.512, -20.000 -24.969 -1.596, -19.000 -24.969 -1.596, -19.000 -4.637 16.896, -20.000 -4.006 14.949, -19.000 -2.013 14.771, -20.000 -2.000 14.761, -19.000 -2.000 14.761, -20.000 -2.000 -14.929, -19.000 -2.000 -14.929, -19.000 -2.001 -14.930, -20.000 -4.001 -15.000, -19.000 -4.000 -17.000, -19.000 -18.255 -16.886, -19.000 -20.801 -12.766, -19.000 -20.286 -12.356, -19.000 -19.682 -12.095, -19.000 -19.031 -12.000, -19.000 -18.378 -12.079, -19.000 -17.768 -12.325, -19.000 -17.243 -12.721, -19.000 -5.406 -16.422, -19.000 -17.189 -16.224, -19.000 -17.680 -16.623, -19.000 -5.724 -16.013, -19.000 -16.815 -15.714, -19.000 -4.993 -16.736, -19.000 -5.927 -15.536, -19.000 -16.580 -15.127, -19.000 -4.514 -16.933, -19.000 -6.000 -15.023, -19.000 -16.500 -14.500, -19.000 -5.939 -14.509, -19.000 -16.587 -13.848, -19.000 -5.747 -14.027, -19.000 -16.840 -13.241, -19.000 -5.439 -13.611, -19.000 -2.086 -14.419, -19.000 -5.709 13.961, -19.000 -5.390 13.562, -19.000 -5.916 14.428, -19.000 -5.999 14.932, -19.000 -5.951 15.441, -19.000 -3.426 13.084, -19.000 -3.531 -13.056, -19.000 -3.047 -13.241, -19.000 -4.000 13.000, -19.000 -5.775 15.921, -19.000 -2.900 13.330, -19.000 -2.628 -13.545, -19.000 -5.484 16.340, -19.000 -2.466 13.716, -19.000 -2.300 -13.947, -19.000 -5.096 16.673, -19.000 -2.162 14.211, -19.000 -15.384 -0.051, -19.000 -4.047 -13.001, -19.000 -4.559 -13.080, -19.000 -5.033 -13.288, -19.000 -4.980 13.257, -19.000 -4.507 13.065, -19.000 -21.426 -13.897, -20.000 -21.430 -13.912, -19.000 -21.500 -14.539, -20.000 -21.499 -14.570, -19.000 -21.406 -15.178, -20.000 -21.393 -15.222, -19.000 -21.152 -15.772, -20.000 -21.120 -15.825, -19.000 -20.754 -16.281, -20.000 -20.699 -16.334, -19.000 -20.239 -16.672, -20.000 -20.158 -16.716, -19.000 -19.641 -16.916, -20.000 -19.537 -16.942, -19.000 -19.000 -17.000, -19.000 -18.959 -17.000, -19.000 -18.918 -16.999, -20.000 -20.158 -16.716, -20.000 -19.537 -16.942, -20.000 -21.430 -13.912, -20.000 -2.073 15.535, -19.000 -2.000 15.000, -20.000 -2.000 15.000, -19.000 -2.073 15.535, -20.000 -2.286 16.031, -19.000 -2.286 16.031, -20.000 -2.624 16.451, -19.000 -2.624 16.451, -20.000 -3.062 16.766, -19.000 -3.062 16.766, -20.000 -3.569 16.953, -19.000 -3.569 16.953, -20.000 -4.106 16.997, -19.000 -4.106 16.997, -20.000 -2.006 14.847, -20.000 -3.318 15.884, -20.000 -2.001 14.924, -20.000 -2.000 15.000, -20.000 -2.006 14.847, -19.000 -2.006 14.847, -19.000 -2.001 14.924, -20.000 -2.000 -14.977, -19.000 -2.000 -15.000, -20.000 -2.000 -15.000, -19.000 -2.000 -14.977, -20.000 -2.001 -14.953, -19.000 -2.001 -14.953, -20.000 -3.482 -16.932, -20.000 -3.000 -16.732, -20.000 -2.586 -16.414, -20.000 -2.268 -16.000, -20.000 -2.068 -15.518, -20.000 -2.000 -15.000, -20.000 -2.000 -14.977, -20.000 -2.001 -14.953, -19.000 -3.482 -16.932, -19.000 -3.000 -16.732, -19.000 -2.586 -16.414, -19.000 -2.268 -16.000, -20.000 -2.268 -16.000, -19.000 -2.068 -15.518, -19.000 -4.000 -15.000, -19.000 -19.000 -14.500, -19.000 -3.999 14.999 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 3, 4, 5, -1, 3, 6, 4, -1, 7, 8, 9, -1, 10, 11, 12, -1, 7, 0, 8, -1, 13, 5, 14, -1, 13, 3, 5, -1, 15, 11, 10, -1, 16, 9, 17, -1, 16, 7, 9, -1, 18, 13, 14, -1, 19, 10, 20, -1, 21, 13, 18, -1, 11, 17, 12, -1, 19, 15, 10, -1, 11, 16, 17, -1, 22, 21, 18, -1, 23, 21, 22, -1, 24, 20, 25, -1, 24, 19, 20, -1, 26, 22, 27, -1, 26, 23, 22, -1, 28, 25, 29, -1, 28, 24, 25, -1, 30, 27, 31, -1, 30, 26, 27, -1, 32, 29, 33, -1, 34, 31, 35, -1, 32, 28, 29, -1, 34, 30, 31, -1, 36, 33, 37, -1, 38, 35, 39, -1, 38, 34, 35, -1, 36, 32, 33, -1, 40, 37, 41, -1, 42, 39, 43, -1, 42, 38, 39, -1, 40, 36, 37, -1, 44, 43, 45, -1, 46, 41, 47, -1, 44, 42, 43, -1, 46, 40, 41, -1, 1, 45, 2, -1, 1, 44, 45, -1, 6, 47, 4, -1, 6, 46, 47, -1, 0, 2, 8, -1, 39, 35, 48, -1, 4, 49, 50, -1, 10, 51, 20, -1, 47, 49, 4, -1, 43, 39, 48, -1, 5, 4, 50, -1, 12, 51, 10, -1, 41, 49, 47, -1, 14, 5, 50, -1, 37, 49, 41, -1, 18, 14, 50, -1, 52, 45, 43, -1, 52, 2, 45, -1, 52, 8, 2, -1, 52, 9, 8, -1, 52, 17, 9, -1, 52, 12, 17, -1, 33, 49, 37, -1, 52, 43, 48, -1, 52, 51, 12, -1, 51, 49, 33, -1, 29, 51, 33, -1, 25, 51, 29, -1, 48, 22, 18, -1, 48, 27, 22, -1, 48, 31, 27, -1, 48, 18, 50, -1, 35, 31, 48, -1, 20, 51, 25, -1, 53, 34, 38, -1, 54, 55, 6, -1, 19, 56, 15, -1, 53, 38, 42, -1, 54, 6, 3, -1, 6, 55, 46, -1, 15, 56, 11, -1, 46, 55, 40, -1, 54, 3, 13, -1, 40, 55, 36, -1, 54, 13, 21, -1, 42, 44, 57, -1, 44, 1, 57, -1, 1, 0, 57, -1, 0, 7, 57, -1, 7, 16, 57, -1, 16, 11, 57, -1, 36, 55, 32, -1, 53, 42, 57, -1, 11, 56, 57, -1, 32, 55, 56, -1, 32, 56, 28, -1, 28, 56, 24, -1, 21, 23, 53, -1, 23, 26, 53, -1, 26, 30, 53, -1, 54, 21, 53, -1, 53, 30, 34, -1, 24, 56, 19, -1, 49, 58, 59, -1, 49, 59, 50, -1, 49, 60, 58, -1, 49, 51, 60, -1, 50, 54, 48, -1, 48, 54, 53, -1, 57, 51, 52, -1, 57, 56, 51, -1, 61, 52, 48, -1, 61, 62, 52, -1, 56, 63, 64, -1, 55, 65, 66, -1, 67, 64, 68, -1, 69, 70, 65, -1, 71, 56, 55, -1, 71, 72, 73, -1, 71, 74, 72, -1, 71, 75, 74, -1, 71, 66, 75, -1, 71, 55, 66, -1, 71, 73, 56, -1, 64, 76, 68, -1, 64, 77, 76, -1, 64, 78, 77, -1, 64, 63, 78, -1, 65, 79, 66, -1, 65, 80, 79, -1, 65, 81, 80, -1, 65, 82, 81, -1, 65, 70, 82, -1, 56, 73, 63, -1, 83, 84, 85, -1, 84, 86, 85, -1, 87, 88, 89, -1, 85, 86, 90, -1, 91, 85, 90, -1, 85, 53, 92, -1, 53, 57, 92, -1, 93, 94, 92, -1, 94, 95, 92, -1, 95, 96, 92, -1, 96, 97, 92, -1, 97, 83, 92, -1, 57, 93, 92, -1, 83, 85, 92, -1, 87, 98, 88, -1, 98, 99, 88, -1, 99, 100, 88, -1, 100, 101, 88, -1, 101, 102, 57, -1, 102, 93, 57, -1, 88, 101, 57, -1, 54, 103, 55, -1, 103, 104, 55, -1, 103, 59, 58, -1, 103, 58, 104, -1, 50, 59, 54, -1, 59, 103, 54, -1, 104, 58, 60, -1, 104, 60, 105, -1, 60, 106, 107, -1, 51, 106, 60, -1, 48, 53, 108, -1, 61, 48, 108, -1, 64, 106, 51, -1, 64, 51, 56, -1, 52, 109, 88, -1, 57, 52, 88, -1, 62, 61, 108, -1, 110, 62, 108, -1, 62, 109, 52, -1, 62, 111, 109, -1, 65, 55, 104, -1, 112, 65, 104, -1, 113, 64, 67, -1, 113, 67, 114, -1, 115, 116, 117, -1, 115, 118, 116, -1, 119, 120, 121, -1, 119, 121, 122, -1, 123, 124, 125, -1, 123, 125, 126, -1, 123, 126, 127, -1, 128, 127, 129, -1, 128, 123, 127, -1, 130, 129, 131, -1, 132, 117, 133, -1, 132, 115, 117, -1, 130, 128, 129, -1, 134, 130, 131, -1, 135, 120, 119, -1, 136, 132, 133, -1, 137, 130, 134, -1, 138, 139, 120, -1, 138, 120, 135, -1, 140, 137, 134, -1, 141, 136, 133, -1, 141, 133, 142, -1, 143, 137, 140, -1, 144, 145, 139, -1, 146, 147, 116, -1, 146, 148, 147, -1, 144, 139, 138, -1, 149, 141, 142, -1, 118, 146, 116, -1, 149, 142, 150, -1, 151, 149, 150, -1, 151, 150, 145, -1, 151, 145, 144, -1, 122, 121, 143, -1, 122, 143, 140, -1, 152, 153, 154, -1, 152, 154, 155, -1, 156, 108, 53, -1, 85, 156, 53, -1, 88, 157, 158, -1, 88, 158, 89, -1, 159, 160, 161, -1, 162, 159, 161, -1, 163, 164, 165, -1, 166, 163, 165, -1, 167, 166, 165, -1, 168, 167, 169, -1, 170, 171, 172, -1, 167, 165, 169, -1, 171, 173, 172, -1, 160, 174, 175, -1, 176, 168, 177, -1, 168, 169, 177, -1, 161, 160, 175, -1, 178, 170, 179, -1, 170, 172, 179, -1, 176, 177, 180, -1, 175, 174, 181, -1, 180, 177, 182, -1, 183, 184, 185, -1, 178, 179, 186, -1, 174, 187, 188, -1, 182, 159, 162, -1, 181, 174, 188, -1, 178, 186, 189, -1, 180, 182, 162, -1, 190, 178, 189, -1, 187, 191, 192, -1, 188, 187, 192, -1, 185, 184, 193, -1, 190, 189, 194, -1, 171, 185, 173, -1, 195, 190, 194, -1, 185, 193, 173, -1, 192, 191, 196, -1, 191, 195, 196, -1, 195, 194, 196, -1, 197, 198, 199, -1, 200, 197, 199, -1, 60, 107, 113, -1, 105, 60, 113, -1, 201, 202, 105, -1, 113, 201, 105, -1, 203, 112, 104, -1, 204, 113, 114, -1, 112, 205, 206, -1, 202, 207, 208, -1, 207, 209, 208, -1, 209, 210, 208, -1, 210, 203, 208, -1, 104, 105, 208, -1, 203, 104, 208, -1, 105, 202, 208, -1, 204, 211, 113, -1, 211, 212, 113, -1, 212, 213, 113, -1, 213, 201, 113, -1, 203, 214, 112, -1, 214, 215, 112, -1, 215, 216, 112, -1, 216, 217, 112, -1, 217, 205, 112, -1, 113, 107, 106, -1, 64, 113, 106, -1, 88, 109, 111, -1, 157, 88, 111, -1, 157, 111, 62, -1, 157, 62, 110, -1, 156, 218, 219, -1, 156, 220, 218, -1, 158, 157, 221, -1, 222, 220, 156, -1, 222, 156, 223, -1, 224, 225, 226, -1, 224, 227, 225, -1, 224, 228, 227, -1, 224, 229, 228, -1, 224, 219, 229, -1, 224, 108, 156, -1, 224, 110, 108, -1, 224, 226, 110, -1, 224, 156, 219, -1, 157, 230, 221, -1, 157, 231, 230, -1, 157, 232, 231, -1, 157, 233, 232, -1, 110, 234, 233, -1, 110, 226, 234, -1, 110, 233, 157, -1, 235, 236, 237, -1, 235, 237, 238, -1, 125, 124, 239, -1, 126, 125, 239, -1, 127, 126, 239, -1, 240, 241, 242, -1, 240, 242, 243, -1, 244, 241, 240, -1, 148, 244, 240, -1, 245, 244, 148, -1, 118, 245, 148, -1, 148, 118, 146, -1, 246, 247, 248, -1, 249, 248, 250, -1, 249, 246, 248, -1, 251, 252, 247, -1, 251, 247, 246, -1, 253, 250, 254, -1, 253, 249, 250, -1, 255, 256, 252, -1, 255, 252, 251, -1, 123, 254, 124, -1, 123, 253, 254, -1, 243, 242, 256, -1, 243, 256, 255, -1, 257, 258, 259, -1, 260, 261, 262, -1, 260, 263, 261, -1, 264, 258, 257, -1, 265, 260, 262, -1, 266, 264, 257, -1, 267, 260, 265, -1, 268, 265, 269, -1, 270, 264, 266, -1, 268, 267, 265, -1, 271, 270, 266, -1, 272, 270, 271, -1, 273, 272, 271, -1, 274, 269, 275, -1, 274, 268, 269, -1, 276, 272, 273, -1, 277, 276, 273, -1, 278, 275, 279, -1, 280, 276, 277, -1, 278, 274, 275, -1, 281, 280, 277, -1, 282, 279, 283, -1, 284, 280, 281, -1, 282, 278, 279, -1, 285, 284, 281, -1, 286, 284, 285, -1, 287, 283, 288, -1, 287, 282, 283, -1, 289, 286, 285, -1, 290, 286, 289, -1, 291, 288, 292, -1, 291, 287, 288, -1, 293, 290, 289, -1, 293, 289, 294, -1, 295, 292, 296, -1, 295, 291, 292, -1, 297, 294, 298, -1, 297, 293, 294, -1, 299, 296, 300, -1, 301, 298, 302, -1, 299, 295, 296, -1, 301, 297, 298, -1, 258, 300, 259, -1, 263, 302, 261, -1, 263, 301, 302, -1, 258, 299, 300, -1, 303, 304, 305, -1, 304, 306, 305, -1, 153, 152, 307, -1, 307, 152, 308, -1, 309, 307, 310, -1, 307, 308, 310, -1, 311, 309, 312, -1, 309, 310, 312, -1, 313, 311, 314, -1, 311, 312, 314, -1, 315, 313, 316, -1, 313, 314, 316, -1, 317, 315, 318, -1, 315, 316, 318, -1, 319, 317, 320, -1, 317, 318, 320, -1, 321, 319, 322, -1, 319, 320, 322, -1, 323, 321, 324, -1, 321, 322, 324, -1, 304, 323, 306, -1, 323, 324, 306, -1, 325, 326, 327, -1, 325, 327, 328, -1, 329, 164, 163, -1, 329, 163, 166, -1, 329, 166, 167, -1, 330, 331, 184, -1, 332, 333, 184, -1, 334, 332, 184, -1, 331, 334, 184, -1, 184, 333, 335, -1, 184, 335, 173, -1, 184, 183, 330, -1, 331, 336, 337, -1, 336, 338, 337, -1, 339, 334, 337, -1, 340, 339, 337, -1, 338, 340, 337, -1, 334, 331, 337, -1, 193, 173, 184, -1, 341, 342, 343, -1, 342, 344, 345, -1, 343, 342, 345, -1, 346, 341, 347, -1, 341, 343, 347, -1, 344, 340, 338, -1, 345, 344, 338, -1, 164, 346, 165, -1, 346, 347, 165, -1, 348, 349, 350, -1, 351, 352, 353, -1, 351, 354, 352, -1, 355, 348, 350, -1, 356, 348, 355, -1, 357, 353, 358, -1, 359, 360, 361, -1, 359, 362, 360, -1, 360, 356, 355, -1, 357, 351, 353, -1, 362, 356, 360, -1, 363, 358, 364, -1, 365, 361, 366, -1, 363, 357, 358, -1, 365, 359, 361, -1, 367, 363, 364, -1, 368, 366, 369, -1, 367, 364, 370, -1, 368, 365, 366, -1, 371, 367, 370, -1, 371, 370, 372, -1, 373, 369, 374, -1, 373, 368, 369, -1, 375, 371, 372, -1, 375, 372, 376, -1, 377, 374, 378, -1, 377, 373, 374, -1, 379, 375, 376, -1, 379, 376, 380, -1, 381, 378, 382, -1, 381, 377, 378, -1, 383, 379, 380, -1, 383, 380, 384, -1, 385, 382, 386, -1, 385, 381, 382, -1, 387, 383, 384, -1, 387, 384, 388, -1, 389, 387, 388, -1, 390, 386, 391, -1, 392, 387, 389, -1, 390, 385, 386, -1, 349, 389, 393, -1, 354, 391, 352, -1, 349, 392, 389, -1, 354, 390, 391, -1, 350, 349, 393, -1, 394, 395, 396, -1, 395, 397, 396, -1, 197, 200, 398, -1, 398, 200, 399, -1, 400, 398, 401, -1, 398, 399, 401, -1, 402, 400, 403, -1, 400, 401, 403, -1, 404, 402, 405, -1, 402, 403, 405, -1, 406, 404, 407, -1, 404, 405, 407, -1, 408, 406, 409, -1, 406, 407, 409, -1, 410, 408, 411, -1, 408, 409, 411, -1, 412, 410, 413, -1, 410, 411, 413, -1, 414, 412, 415, -1, 412, 413, 415, -1, 395, 414, 397, -1, 414, 415, 397, -1, 291, 295, 416, -1, 295, 152, 416, -1, 260, 267, 417, -1, 418, 260, 417, -1, 419, 305, 290, -1, 420, 419, 290, -1, 235, 420, 290, -1, 421, 235, 293, -1, 235, 290, 293, -1, 422, 421, 297, -1, 287, 291, 423, -1, 291, 416, 423, -1, 421, 293, 297, -1, 422, 297, 424, -1, 417, 267, 425, -1, 287, 423, 426, -1, 424, 297, 301, -1, 267, 268, 427, -1, 425, 267, 427, -1, 424, 301, 428, -1, 287, 426, 429, -1, 282, 287, 429, -1, 428, 301, 263, -1, 268, 274, 430, -1, 295, 299, 431, -1, 299, 258, 431, -1, 427, 268, 430, -1, 282, 429, 432, -1, 295, 431, 152, -1, 278, 282, 432, -1, 278, 432, 433, -1, 274, 278, 433, -1, 430, 274, 433, -1, 263, 260, 418, -1, 428, 263, 418, -1, 434, 355, 350, -1, 434, 350, 435, -1, 384, 394, 436, -1, 384, 436, 437, -1, 384, 437, 326, -1, 388, 326, 438, -1, 439, 386, 382, -1, 388, 384, 326, -1, 439, 197, 386, -1, 389, 438, 440, -1, 441, 360, 355, -1, 389, 388, 438, -1, 441, 355, 434, -1, 442, 382, 378, -1, 442, 439, 382, -1, 443, 389, 440, -1, 444, 360, 441, -1, 393, 389, 443, -1, 391, 445, 352, -1, 446, 442, 378, -1, 447, 361, 360, -1, 435, 350, 393, -1, 447, 360, 444, -1, 448, 446, 378, -1, 435, 393, 443, -1, 448, 378, 374, -1, 449, 366, 361, -1, 449, 361, 447, -1, 450, 445, 391, -1, 451, 448, 374, -1, 197, 391, 386, -1, 451, 374, 369, -1, 197, 450, 391, -1, 452, 366, 449, -1, 452, 451, 369, -1, 452, 369, 366, -1, 238, 305, 419, -1, 238, 419, 420, -1, 238, 420, 235, -1, 305, 237, 303, -1, 238, 237, 305, -1, 264, 312, 310, -1, 264, 310, 258, -1, 258, 310, 308, -1, 258, 308, 152, -1, 258, 431, 152, -1, 322, 320, 280, -1, 324, 322, 284, -1, 322, 280, 284, -1, 320, 318, 276, -1, 280, 320, 276, -1, 306, 324, 286, -1, 324, 284, 286, -1, 318, 316, 272, -1, 276, 318, 272, -1, 305, 306, 290, -1, 306, 286, 290, -1, 316, 314, 270, -1, 272, 316, 270, -1, 314, 312, 264, -1, 270, 314, 264, -1, 436, 394, 327, -1, 437, 436, 327, -1, 326, 437, 327, -1, 396, 327, 394, -1, 328, 327, 396, -1, 445, 400, 402, -1, 445, 402, 404, -1, 445, 358, 353, -1, 445, 404, 358, -1, 352, 445, 353, -1, 398, 400, 445, -1, 197, 398, 352, -1, 445, 450, 414, -1, 453, 404, 406, -1, 453, 406, 408, -1, 453, 364, 358, -1, 453, 370, 364, -1, 453, 408, 370, -1, 453, 358, 404, -1, 376, 412, 414, -1, 372, 410, 412, -1, 372, 412, 376, -1, 380, 414, 395, -1, 380, 376, 414, -1, 370, 408, 410, -1, 370, 410, 372, -1, 384, 395, 394, -1, 384, 380, 395, -1, 454, 455, 456, -1, 457, 454, 456, -1, 458, 457, 456, -1, 459, 458, 456, -1, 460, 459, 456, -1, 461, 460, 456, -1, 455, 462, 463, -1, 464, 465, 466, -1, 467, 468, 466, -1, 469, 467, 466, -1, 470, 469, 466, -1, 471, 470, 466, -1, 472, 471, 466, -1, 473, 472, 466, -1, 474, 473, 466, -1, 475, 474, 466, -1, 463, 475, 466, -1, 476, 461, 466, -1, 477, 476, 466, -1, 478, 477, 466, -1, 479, 478, 466, -1, 480, 479, 466, -1, 468, 480, 466, -1, 461, 464, 466, -1, 481, 463, 466, -1, 465, 481, 466, -1, 482, 463, 483, -1, 482, 484, 463, -1, 485, 483, 486, -1, 485, 482, 483, -1, 487, 486, 488, -1, 487, 485, 486, -1, 489, 488, 490, -1, 489, 487, 488, -1, 491, 490, 492, -1, 491, 489, 490, -1, 493, 492, 494, -1, 493, 491, 492, -1, 495, 494, 496, -1, 495, 493, 494, -1, 497, 496, 461, -1, 497, 495, 496, -1, 498, 499, 500, -1, 501, 502, 503, -1, 503, 502, 504, -1, 505, 503, 504, -1, 505, 504, 506, -1, 507, 506, 508, -1, 506, 504, 508, -1, 507, 508, 509, -1, 510, 509, 511, -1, 509, 508, 511, -1, 512, 510, 513, -1, 510, 511, 513, -1, 514, 461, 476, -1, 514, 476, 515, -1, 515, 476, 477, -1, 516, 515, 478, -1, 515, 477, 478, -1, 516, 478, 479, -1, 517, 516, 480, -1, 516, 479, 480, -1, 518, 519, 520, -1, 519, 521, 522, -1, 520, 519, 522, -1, 500, 518, 523, -1, 518, 520, 523, -1, 521, 524, 525, -1, 522, 521, 525, -1, 500, 523, 526, -1, 498, 500, 526, -1, 524, 527, 528, -1, 525, 524, 528, -1, 498, 526, 529, -1, 527, 530, 531, -1, 528, 527, 531, -1, 530, 532, 533, -1, 531, 530, 533, -1, 471, 472, 534, -1, 472, 473, 534, -1, 473, 474, 534, -1, 474, 475, 534, -1, 475, 463, 534, -1, 532, 512, 535, -1, 533, 532, 535, -1, 536, 537, 538, -1, 469, 470, 539, -1, 470, 471, 539, -1, 471, 534, 539, -1, 469, 539, 540, -1, 541, 542, 543, -1, 542, 544, 543, -1, 543, 544, 545, -1, 545, 544, 546, -1, 546, 544, 547, -1, 548, 549, 550, -1, 551, 548, 550, -1, 547, 544, 552, -1, 551, 550, 553, -1, 554, 551, 553, -1, 552, 544, 555, -1, 554, 553, 556, -1, 557, 554, 556, -1, 555, 544, 558, -1, 557, 556, 559, -1, 538, 557, 559, -1, 558, 544, 560, -1, 558, 560, 561, -1, 559, 562, 563, -1, 538, 563, 536, -1, 559, 563, 538, -1, 513, 564, 565, -1, 564, 517, 565, -1, 542, 541, 565, -1, 480, 468, 565, -1, 468, 467, 565, -1, 467, 469, 565, -1, 512, 513, 565, -1, 566, 542, 565, -1, 540, 566, 565, -1, 549, 548, 565, -1, 541, 567, 565, -1, 567, 568, 565, -1, 568, 569, 565, -1, 517, 480, 565, -1, 569, 549, 565, -1, 469, 540, 565, -1, 548, 570, 565, -1, 570, 571, 565, -1, 571, 572, 565, -1, 572, 535, 565, -1, 535, 512, 565, -1, 573, 574, 575, -1, 573, 575, 576, -1, 573, 576, 577, -1, 573, 577, 578, -1, 573, 578, 579, -1, 573, 579, 580, -1, 573, 580, 581, -1, 573, 581, 582, -1, 573, 582, 484, -1, 573, 583, 584, -1, 573, 584, 585, -1, 573, 585, 586, -1, 573, 586, 587, -1, 573, 587, 588, -1, 573, 588, 574, -1, 573, 497, 589, -1, 573, 589, 590, -1, 573, 590, 583, -1, 573, 484, 482, -1, 573, 482, 485, -1, 573, 485, 487, -1, 573, 487, 489, -1, 573, 489, 491, -1, 573, 491, 493, -1, 573, 493, 495, -1, 573, 495, 497, -1, 591, 461, 592, -1, 497, 461, 591, -1, 593, 594, 595, -1, 593, 595, 544, -1, 596, 544, 597, -1, 596, 593, 544, -1, 598, 597, 566, -1, 598, 596, 597, -1, 599, 566, 600, -1, 599, 598, 566, -1, 601, 600, 602, -1, 601, 599, 600, -1, 603, 602, 604, -1, 603, 601, 602, -1, 484, 604, 463, -1, 484, 603, 604, -1, 605, 606, 607, -1, 605, 607, 608, -1, 499, 501, 609, -1, 530, 527, 609, -1, 532, 530, 609, -1, 512, 532, 609, -1, 510, 512, 609, -1, 509, 510, 609, -1, 507, 509, 609, -1, 506, 507, 609, -1, 505, 506, 609, -1, 503, 505, 609, -1, 501, 503, 609, -1, 500, 499, 609, -1, 518, 500, 609, -1, 519, 518, 609, -1, 521, 519, 609, -1, 524, 521, 609, -1, 527, 524, 609, -1, 610, 502, 611, -1, 610, 611, 612, -1, 502, 610, 613, -1, 504, 502, 613, -1, 614, 504, 615, -1, 504, 613, 615, -1, 616, 614, 617, -1, 614, 615, 617, -1, 616, 617, 618, -1, 618, 617, 619, -1, 618, 619, 620, -1, 620, 619, 621, -1, 620, 621, 517, -1, 517, 621, 622, -1, 517, 622, 516, -1, 516, 622, 623, -1, 516, 623, 515, -1, 515, 623, 624, -1, 515, 624, 592, -1, 592, 624, 591, -1, 594, 625, 595, -1, 626, 625, 594, -1, 558, 561, 627, -1, 555, 558, 627, -1, 552, 555, 627, -1, 547, 552, 627, -1, 546, 547, 627, -1, 545, 546, 627, -1, 543, 545, 627, -1, 541, 543, 627, -1, 567, 541, 627, -1, 568, 567, 627, -1, 569, 568, 627, -1, 549, 569, 627, -1, 550, 549, 627, -1, 553, 550, 627, -1, 556, 553, 627, -1, 559, 556, 627, -1, 562, 559, 627, -1, 561, 562, 627, -1, 628, 563, 629, -1, 630, 563, 628, -1, 536, 563, 630, -1, 631, 536, 630, -1, 632, 537, 536, -1, 632, 536, 631, -1, 551, 554, 633, -1, 548, 551, 633, -1, 570, 548, 633, -1, 571, 570, 633, -1, 572, 571, 633, -1, 535, 572, 633, -1, 533, 535, 633, -1, 531, 533, 633, -1, 528, 531, 633, -1, 525, 528, 633, -1, 522, 525, 633, -1, 520, 522, 633, -1, 523, 520, 633, -1, 526, 523, 633, -1, 529, 526, 633, -1, 537, 529, 633, -1, 538, 537, 633, -1, 557, 538, 633, -1, 554, 557, 633, -1, 608, 607, 529, -1, 608, 529, 634, -1, 635, 605, 608, -1, 636, 610, 612, -1, 613, 610, 636, -1, 613, 636, 637, -1, 638, 613, 637, -1, 615, 613, 638, -1, 615, 638, 639, -1, 640, 615, 639, -1, 617, 615, 640, -1, 617, 640, 641, -1, 589, 497, 591, -1, 590, 589, 591, -1, 583, 590, 591, -1, 619, 641, 642, -1, 619, 617, 641, -1, 624, 583, 591, -1, 624, 584, 583, -1, 585, 584, 624, -1, 586, 585, 624, -1, 586, 624, 623, -1, 587, 586, 623, -1, 588, 587, 623, -1, 588, 623, 622, -1, 643, 644, 645, -1, 646, 647, 644, -1, 646, 644, 643, -1, 648, 645, 635, -1, 648, 643, 645, -1, 649, 650, 647, -1, 649, 647, 646, -1, 651, 648, 635, -1, 651, 635, 608, -1, 652, 653, 650, -1, 652, 650, 649, -1, 634, 651, 608, -1, 654, 655, 653, -1, 654, 653, 652, -1, 656, 657, 655, -1, 656, 655, 654, -1, 603, 579, 578, -1, 603, 580, 579, -1, 603, 581, 580, -1, 603, 582, 581, -1, 603, 484, 582, -1, 658, 657, 656, -1, 658, 642, 657, -1, 659, 632, 631, -1, 601, 603, 578, -1, 601, 577, 576, -1, 601, 578, 577, -1, 599, 601, 576, -1, 660, 596, 661, -1, 660, 593, 596, -1, 662, 593, 660, -1, 663, 593, 662, -1, 664, 593, 663, -1, 665, 666, 667, -1, 665, 668, 666, -1, 669, 593, 664, -1, 670, 667, 671, -1, 670, 665, 667, -1, 672, 593, 669, -1, 673, 671, 674, -1, 673, 670, 671, -1, 675, 593, 672, -1, 676, 674, 659, -1, 676, 673, 674, -1, 594, 593, 675, -1, 626, 594, 675, -1, 630, 628, 676, -1, 631, 630, 659, -1, 659, 630, 676, -1, 677, 642, 658, -1, 677, 678, 666, -1, 677, 679, 678, -1, 677, 680, 679, -1, 677, 658, 680, -1, 677, 599, 576, -1, 677, 619, 642, -1, 677, 661, 596, -1, 677, 681, 661, -1, 677, 682, 681, -1, 677, 668, 682, -1, 677, 596, 598, -1, 677, 598, 599, -1, 677, 588, 622, -1, 677, 621, 619, -1, 677, 622, 621, -1, 677, 575, 574, -1, 677, 576, 575, -1, 677, 574, 588, -1, 677, 666, 668, -1, 683, 611, 684, -1, 683, 612, 611, -1, 685, 683, 684, -1, 686, 685, 684, -1, 687, 685, 686, -1, 688, 687, 686, -1, 689, 687, 688, -1, 690, 689, 688, -1, 691, 689, 690, -1, 692, 691, 690, -1, 693, 691, 692, -1, 694, 693, 692, -1, 695, 693, 694, -1, 696, 695, 694, -1, 697, 695, 696, -1, 606, 698, 697, -1, 606, 699, 698, -1, 606, 697, 696, -1, 605, 699, 606, -1, 692, 690, 499, -1, 700, 692, 499, -1, 701, 700, 499, -1, 702, 703, 501, -1, 704, 702, 501, -1, 690, 704, 501, -1, 499, 690, 501, -1, 705, 706, 707, -1, 708, 706, 705, -1, 709, 708, 705, -1, 710, 708, 709, -1, 711, 710, 709, -1, 712, 710, 711, -1, 713, 712, 711, -1, 714, 712, 713, -1, 715, 714, 713, -1, 716, 714, 715, -1, 717, 716, 715, -1, 718, 716, 717, -1, 625, 718, 717, -1, 626, 718, 625, -1, 719, 562, 720, -1, 721, 719, 720, -1, 707, 721, 720, -1, 562, 561, 720, -1, 705, 707, 720, -1, 722, 705, 720, -1, 723, 722, 720, -1, 724, 723, 720, -1, 715, 724, 720, -1, 717, 715, 720, -1, 561, 717, 720, -1, 725, 628, 629, -1, 726, 628, 725, -1, 727, 726, 725, -1, 728, 726, 727, -1, 707, 728, 727, -1, 706, 728, 707, -1, 729, 730, 731, -1, 732, 730, 729, -1, 733, 732, 729, -1, 734, 732, 733, -1, 537, 734, 733, -1, 632, 734, 537, -1, 735, 529, 537, -1, 736, 735, 537, -1, 737, 736, 537, -1, 738, 737, 537, -1, 739, 738, 537, -1, 731, 739, 537, -1, 729, 731, 537, -1, 733, 729, 537, -1, 740, 529, 735, -1, 740, 634, 529, -1, 741, 735, 742, -1, 741, 740, 735, -1, 743, 742, 744, -1, 743, 741, 742, -1, 745, 744, 738, -1, 745, 743, 744, -1, 746, 738, 739, -1, 746, 745, 738, -1, 731, 746, 739, -1, 730, 746, 731, -1, 747, 730, 732, -1, 747, 732, 734, -1, 747, 734, 632, -1, 747, 634, 740, -1, 747, 740, 741, -1, 747, 741, 743, -1, 747, 743, 745, -1, 747, 745, 746, -1, 747, 746, 730, -1, 747, 632, 659, -1, 747, 659, 674, -1, 747, 674, 671, -1, 747, 671, 667, -1, 747, 667, 666, -1, 747, 666, 678, -1, 747, 678, 679, -1, 747, 679, 680, -1, 747, 680, 658, -1, 747, 658, 656, -1, 747, 656, 654, -1, 747, 654, 652, -1, 747, 652, 649, -1, 747, 649, 646, -1, 747, 646, 643, -1, 747, 643, 648, -1, 747, 648, 651, -1, 747, 651, 634, -1, 748, 653, 655, -1, 748, 655, 657, -1, 748, 657, 642, -1, 748, 642, 641, -1, 748, 641, 640, -1, 748, 640, 639, -1, 748, 639, 638, -1, 748, 638, 637, -1, 748, 637, 636, -1, 748, 636, 612, -1, 748, 605, 635, -1, 748, 635, 645, -1, 748, 645, 644, -1, 748, 644, 647, -1, 748, 647, 650, -1, 748, 650, 653, -1, 748, 697, 698, -1, 748, 698, 699, -1, 748, 699, 605, -1, 748, 612, 683, -1, 748, 683, 685, -1, 748, 685, 687, -1, 748, 687, 689, -1, 748, 689, 691, -1, 748, 691, 693, -1, 748, 693, 695, -1, 748, 695, 697, -1, 749, 706, 708, -1, 749, 708, 710, -1, 749, 710, 712, -1, 749, 712, 714, -1, 749, 714, 716, -1, 749, 716, 718, -1, 749, 718, 626, -1, 749, 628, 726, -1, 749, 726, 728, -1, 749, 728, 706, -1, 749, 668, 665, -1, 749, 665, 670, -1, 749, 670, 673, -1, 749, 673, 676, -1, 749, 676, 628, -1, 749, 626, 675, -1, 749, 675, 672, -1, 749, 672, 669, -1, 749, 669, 664, -1, 749, 664, 663, -1, 749, 663, 662, -1, 749, 662, 660, -1, 749, 660, 661, -1, 749, 661, 681, -1, 749, 681, 682, -1, 749, 682, 668, -1, 750, 751, 752, -1, 753, 750, 752, -1, 754, 753, 752, -1, 755, 754, 752, -1, 756, 755, 752, -1, 757, 756, 752, -1, 751, 758, 759, -1, 760, 761, 762, -1, 763, 764, 762, -1, 765, 763, 762, -1, 766, 765, 762, -1, 767, 766, 762, -1, 768, 767, 762, -1, 769, 768, 762, -1, 770, 769, 762, -1, 771, 770, 762, -1, 759, 771, 762, -1, 772, 757, 762, -1, 773, 772, 762, -1, 774, 773, 762, -1, 775, 774, 762, -1, 776, 775, 762, -1, 764, 776, 762, -1, 757, 760, 762, -1, 777, 759, 762, -1, 761, 777, 762, -1, 778, 759, 758, -1, 778, 779, 759, -1, 780, 758, 781, -1, 780, 778, 758, -1, 782, 781, 750, -1, 782, 780, 781, -1, 783, 750, 753, -1, 783, 782, 750, -1, 784, 753, 754, -1, 784, 783, 753, -1, 785, 754, 786, -1, 785, 784, 754, -1, 787, 786, 756, -1, 787, 785, 786, -1, 788, 756, 757, -1, 788, 787, 756, -1, 789, 790, 791, -1, 792, 793, 794, -1, 794, 793, 795, -1, 796, 794, 795, -1, 796, 795, 797, -1, 798, 797, 799, -1, 797, 795, 799, -1, 798, 799, 800, -1, 801, 800, 802, -1, 800, 799, 802, -1, 803, 801, 804, -1, 801, 802, 804, -1, 805, 757, 772, -1, 805, 772, 806, -1, 806, 772, 773, -1, 807, 806, 774, -1, 806, 773, 774, -1, 807, 774, 775, -1, 808, 807, 776, -1, 807, 775, 776, -1, 809, 810, 811, -1, 810, 812, 813, -1, 811, 810, 813, -1, 791, 809, 814, -1, 809, 811, 814, -1, 812, 815, 816, -1, 813, 812, 816, -1, 791, 814, 817, -1, 789, 791, 817, -1, 815, 818, 819, -1, 816, 815, 819, -1, 789, 817, 820, -1, 818, 821, 822, -1, 819, 818, 822, -1, 821, 823, 824, -1, 822, 821, 824, -1, 767, 768, 825, -1, 768, 769, 825, -1, 769, 770, 825, -1, 770, 771, 825, -1, 771, 759, 825, -1, 823, 803, 826, -1, 824, 823, 826, -1, 827, 828, 829, -1, 765, 766, 830, -1, 766, 767, 830, -1, 767, 825, 830, -1, 765, 830, 831, -1, 832, 833, 834, -1, 833, 835, 834, -1, 834, 835, 836, -1, 836, 835, 837, -1, 837, 835, 838, -1, 839, 840, 841, -1, 842, 839, 841, -1, 838, 835, 843, -1, 842, 841, 844, -1, 845, 842, 844, -1, 843, 835, 846, -1, 845, 844, 847, -1, 848, 845, 847, -1, 846, 835, 849, -1, 848, 847, 850, -1, 829, 848, 850, -1, 849, 835, 851, -1, 849, 851, 852, -1, 850, 853, 854, -1, 829, 854, 827, -1, 850, 854, 829, -1, 833, 832, 855, -1, 804, 856, 855, -1, 856, 808, 855, -1, 765, 831, 855, -1, 776, 764, 855, -1, 764, 763, 855, -1, 763, 765, 855, -1, 803, 804, 855, -1, 857, 833, 855, -1, 831, 857, 855, -1, 840, 839, 855, -1, 832, 858, 855, -1, 858, 859, 855, -1, 859, 860, 855, -1, 808, 776, 855, -1, 860, 840, 855, -1, 839, 861, 855, -1, 861, 862, 855, -1, 862, 863, 855, -1, 863, 826, 855, -1, 826, 803, 855, -1, 864, 865, 866, -1, 864, 866, 867, -1, 864, 867, 868, -1, 864, 868, 869, -1, 864, 869, 870, -1, 864, 870, 871, -1, 864, 871, 872, -1, 864, 872, 873, -1, 864, 873, 779, -1, 864, 874, 875, -1, 864, 875, 876, -1, 864, 876, 877, -1, 864, 877, 878, -1, 864, 878, 879, -1, 864, 879, 865, -1, 864, 788, 880, -1, 864, 880, 881, -1, 864, 881, 874, -1, 864, 779, 778, -1, 864, 778, 780, -1, 864, 780, 782, -1, 864, 782, 783, -1, 864, 783, 784, -1, 864, 784, 785, -1, 864, 785, 787, -1, 864, 787, 788, -1, 882, 757, 805, -1, 788, 757, 882, -1, 883, 884, 851, -1, 883, 851, 835, -1, 885, 835, 886, -1, 885, 883, 835, -1, 887, 886, 857, -1, 887, 885, 886, -1, 888, 857, 831, -1, 888, 887, 857, -1, 889, 831, 830, -1, 889, 888, 831, -1, 890, 830, 825, -1, 890, 889, 830, -1, 779, 825, 759, -1, 779, 890, 825, -1, 891, 892, 789, -1, 891, 789, 893, -1, 790, 792, 894, -1, 821, 818, 894, -1, 823, 821, 894, -1, 803, 823, 894, -1, 801, 803, 894, -1, 800, 801, 894, -1, 798, 800, 894, -1, 797, 798, 894, -1, 796, 797, 894, -1, 794, 796, 894, -1, 792, 794, 894, -1, 791, 790, 894, -1, 809, 791, 894, -1, 810, 809, 894, -1, 812, 810, 894, -1, 815, 812, 894, -1, 818, 815, 894, -1, 895, 793, 792, -1, 895, 792, 896, -1, 793, 895, 897, -1, 795, 793, 897, -1, 898, 795, 899, -1, 795, 897, 899, -1, 900, 898, 901, -1, 898, 899, 901, -1, 900, 901, 804, -1, 804, 901, 902, -1, 804, 902, 856, -1, 856, 902, 903, -1, 856, 903, 808, -1, 808, 903, 904, -1, 808, 904, 807, -1, 807, 904, 905, -1, 807, 905, 906, -1, 906, 905, 907, -1, 906, 907, 805, -1, 805, 907, 882, -1, 884, 852, 851, -1, 908, 852, 884, -1, 849, 852, 909, -1, 846, 849, 909, -1, 843, 846, 909, -1, 838, 843, 909, -1, 837, 838, 909, -1, 836, 837, 909, -1, 834, 836, 909, -1, 832, 834, 909, -1, 858, 832, 909, -1, 859, 858, 909, -1, 860, 859, 909, -1, 840, 860, 909, -1, 841, 840, 909, -1, 844, 841, 909, -1, 847, 844, 909, -1, 850, 847, 909, -1, 853, 850, 909, -1, 852, 853, 909, -1, 910, 911, 853, -1, 912, 911, 910, -1, 913, 911, 912, -1, 914, 913, 912, -1, 915, 828, 913, -1, 915, 913, 914, -1, 842, 845, 916, -1, 839, 842, 916, -1, 861, 839, 916, -1, 862, 861, 916, -1, 863, 862, 916, -1, 826, 863, 916, -1, 824, 826, 916, -1, 822, 824, 916, -1, 819, 822, 916, -1, 816, 819, 916, -1, 813, 816, 916, -1, 811, 813, 916, -1, 814, 811, 916, -1, 817, 814, 916, -1, 820, 817, 916, -1, 828, 820, 916, -1, 829, 828, 916, -1, 848, 829, 916, -1, 845, 848, 916, -1, 893, 789, 820, -1, 893, 820, 917, -1, 918, 891, 893, -1, 919, 895, 896, -1, 897, 895, 919, -1, 897, 919, 920, -1, 921, 897, 920, -1, 899, 897, 921, -1, 899, 921, 922, -1, 923, 899, 922, -1, 901, 899, 923, -1, 901, 923, 924, -1, 880, 788, 882, -1, 881, 880, 882, -1, 874, 881, 882, -1, 902, 924, 925, -1, 902, 901, 924, -1, 907, 874, 882, -1, 907, 875, 874, -1, 876, 875, 907, -1, 877, 876, 907, -1, 877, 907, 905, -1, 878, 877, 905, -1, 879, 878, 905, -1, 879, 905, 904, -1, 926, 927, 928, -1, 929, 930, 927, -1, 929, 927, 926, -1, 931, 928, 918, -1, 931, 926, 928, -1, 932, 933, 930, -1, 932, 930, 929, -1, 934, 931, 918, -1, 934, 918, 893, -1, 935, 936, 933, -1, 935, 933, 932, -1, 917, 934, 893, -1, 937, 938, 936, -1, 937, 936, 935, -1, 939, 940, 938, -1, 939, 938, 937, -1, 890, 870, 869, -1, 890, 871, 870, -1, 890, 872, 871, -1, 890, 873, 872, -1, 890, 779, 873, -1, 941, 940, 939, -1, 941, 925, 940, -1, 942, 915, 914, -1, 889, 890, 869, -1, 889, 868, 867, -1, 889, 869, 868, -1, 888, 889, 867, -1, 943, 885, 944, -1, 943, 883, 885, -1, 945, 883, 943, -1, 946, 883, 945, -1, 947, 883, 946, -1, 948, 949, 950, -1, 948, 951, 949, -1, 952, 883, 947, -1, 953, 950, 954, -1, 953, 948, 950, -1, 955, 883, 952, -1, 956, 954, 957, -1, 956, 953, 954, -1, 958, 883, 955, -1, 959, 957, 942, -1, 959, 956, 957, -1, 884, 883, 958, -1, 908, 884, 958, -1, 912, 910, 959, -1, 914, 912, 942, -1, 942, 912, 959, -1, 960, 925, 941, -1, 960, 961, 949, -1, 960, 962, 961, -1, 960, 963, 962, -1, 960, 941, 963, -1, 960, 888, 867, -1, 960, 902, 925, -1, 960, 944, 885, -1, 960, 964, 944, -1, 960, 965, 964, -1, 960, 951, 965, -1, 960, 885, 887, -1, 960, 887, 888, -1, 960, 879, 904, -1, 960, 903, 902, -1, 960, 904, 903, -1, 960, 866, 865, -1, 960, 867, 866, -1, 960, 865, 879, -1, 960, 949, 951, -1, 966, 792, 967, -1, 966, 896, 792, -1, 968, 966, 967, -1, 969, 968, 967, -1, 970, 968, 969, -1, 971, 970, 969, -1, 972, 970, 971, -1, 973, 972, 971, -1, 974, 972, 973, -1, 975, 974, 973, -1, 976, 974, 975, -1, 977, 976, 975, -1, 978, 976, 977, -1, 979, 978, 977, -1, 980, 978, 979, -1, 892, 981, 980, -1, 892, 982, 981, -1, 892, 980, 979, -1, 891, 982, 892, -1, 975, 973, 790, -1, 983, 975, 790, -1, 984, 983, 790, -1, 969, 985, 792, -1, 971, 969, 792, -1, 973, 971, 792, -1, 790, 973, 792, -1, 986, 987, 988, -1, 989, 987, 986, -1, 990, 989, 986, -1, 991, 989, 990, -1, 992, 991, 990, -1, 993, 991, 992, -1, 994, 993, 992, -1, 995, 993, 994, -1, 996, 995, 994, -1, 997, 995, 996, -1, 998, 997, 996, -1, 999, 997, 998, -1, 852, 999, 998, -1, 908, 999, 852, -1, 1000, 853, 1001, -1, 1002, 1000, 1001, -1, 1003, 1002, 1001, -1, 853, 852, 1001, -1, 986, 1003, 1001, -1, 990, 986, 1001, -1, 992, 990, 1001, -1, 994, 992, 1001, -1, 996, 994, 1001, -1, 998, 996, 1001, -1, 852, 998, 1001, -1, 1004, 910, 853, -1, 1005, 910, 1004, -1, 1002, 1005, 1004, -1, 1006, 1005, 1002, -1, 988, 1006, 1002, -1, 987, 1006, 988, -1, 1007, 1008, 1009, -1, 1010, 1008, 1007, -1, 1011, 1010, 1007, -1, 1012, 1010, 1011, -1, 828, 1012, 1011, -1, 915, 1012, 828, -1, 1013, 820, 828, -1, 1014, 1013, 828, -1, 1015, 1014, 828, -1, 1016, 1015, 828, -1, 1017, 1016, 828, -1, 1018, 1017, 828, -1, 1019, 1018, 828, -1, 1020, 1019, 828, -1, 1021, 820, 1013, -1, 1021, 917, 820, -1, 1022, 1013, 1014, -1, 1022, 1021, 1013, -1, 1023, 1014, 1015, -1, 1023, 1022, 1014, -1, 1024, 1015, 1025, -1, 1024, 1023, 1015, -1, 1026, 1025, 1017, -1, 1026, 1024, 1025, -1, 1009, 1026, 1017, -1, 1008, 1026, 1009, -1, 1027, 1008, 1010, -1, 1027, 1010, 1012, -1, 1027, 1012, 915, -1, 1027, 917, 1021, -1, 1027, 1021, 1022, -1, 1027, 1022, 1023, -1, 1027, 1023, 1024, -1, 1027, 1024, 1026, -1, 1027, 1026, 1008, -1, 1027, 915, 942, -1, 1027, 942, 957, -1, 1027, 957, 954, -1, 1027, 954, 950, -1, 1027, 950, 949, -1, 1027, 949, 961, -1, 1027, 961, 962, -1, 1027, 962, 963, -1, 1027, 963, 941, -1, 1027, 941, 939, -1, 1027, 939, 937, -1, 1027, 937, 935, -1, 1027, 935, 932, -1, 1027, 932, 929, -1, 1027, 929, 926, -1, 1027, 926, 931, -1, 1027, 931, 934, -1, 1027, 934, 917, -1, 1028, 936, 938, -1, 1028, 938, 940, -1, 1028, 940, 925, -1, 1028, 925, 924, -1, 1028, 924, 923, -1, 1028, 923, 922, -1, 1028, 922, 921, -1, 1028, 921, 920, -1, 1028, 920, 919, -1, 1028, 919, 896, -1, 1028, 891, 918, -1, 1028, 918, 928, -1, 1028, 928, 927, -1, 1028, 927, 930, -1, 1028, 930, 933, -1, 1028, 933, 936, -1, 1028, 980, 981, -1, 1028, 981, 982, -1, 1028, 982, 891, -1, 1028, 896, 966, -1, 1028, 966, 968, -1, 1028, 968, 970, -1, 1028, 970, 972, -1, 1028, 972, 974, -1, 1028, 974, 976, -1, 1028, 976, 978, -1, 1028, 978, 980, -1, 1029, 987, 989, -1, 1029, 989, 991, -1, 1029, 991, 993, -1, 1029, 993, 995, -1, 1029, 995, 997, -1, 1029, 997, 999, -1, 1029, 999, 908, -1, 1029, 910, 1005, -1, 1029, 1005, 1006, -1, 1029, 1006, 987, -1, 1029, 951, 948, -1, 1029, 948, 953, -1, 1029, 953, 956, -1, 1029, 956, 959, -1, 1029, 959, 910, -1, 1029, 908, 958, -1, 1029, 958, 955, -1, 1029, 955, 952, -1, 1029, 952, 947, -1, 1029, 947, 946, -1, 1029, 946, 945, -1, 1029, 945, 943, -1, 1029, 943, 944, -1, 1029, 944, 964, -1, 1029, 964, 965, -1, 1029, 965, 951, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -2.000 -17.234 59.209 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -2.000 -17.234 -59.212 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -2.000 -76.444 -0.001 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -2.000 41.977 -0.001 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -61.211 -17.234 -0.001 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 57.211 -17.234 -0.001 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/R_SHOULDER_R.wrl000066400000000000000000001652041207742442300235620ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 37.000 36.299 65.500 center 0.000 0.000 0.000 #translation 0.500 2.150 22.750 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.50 0.50 0.50 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ -16.500 -0.016 0.000, -16.500 -4.000 -0.000, -16.500 -3.874 0.995, -16.500 -3.505 1.927, -16.500 -2.916 2.738, -16.500 -2.143 3.377, -16.500 -1.236 3.804, -16.500 -0.251 3.992, -16.500 0.750 3.929, -16.500 1.703 3.619, -16.500 2.550 3.082, -16.500 3.236 2.351, -16.500 3.719 1.472, -16.500 3.968 0.501, -16.500 3.968 -0.501, -16.500 3.719 -1.472, -16.500 3.236 -2.351, -16.500 2.550 -3.082, -16.500 1.703 -3.619, -16.500 0.750 -3.929, -16.500 -0.251 -3.992, -16.500 -1.236 -3.804, -16.500 -2.143 -3.377, -16.500 -2.916 -2.738, -16.500 -3.505 -1.927, -16.500 -3.874 -0.995, -16.500 -4.000 -0.000, -15.550 -4.000 -0.000, -15.550 -3.864 1.035, -15.550 -3.464 2.000, -15.550 -2.828 2.828, -15.550 -2.000 3.464, -16.500 -1.236 3.804, -15.550 -1.035 3.864, -15.550 0.000 4.000, -16.500 0.750 3.929, -15.550 1.035 3.864, -15.550 2.000 3.464, -16.500 2.550 3.082, -15.550 2.828 2.828, -15.550 3.464 2.000, -16.500 3.719 1.472, -15.550 3.864 1.035, -15.550 4.000 -0.000, -15.550 3.864 -1.035, -16.500 3.719 -1.472, -15.550 3.464 -2.000, -15.550 2.828 -2.828, -16.500 2.550 -3.082, -15.550 2.000 -3.464, -15.550 1.035 -3.864, -16.500 0.750 -3.929, -15.550 -0.000 -4.000, -16.500 -0.251 -3.992, -15.550 -1.035 -3.864, -16.500 -1.236 -3.804, -15.550 -2.000 -3.464, -16.500 -2.143 -3.377, -15.550 -2.828 -2.828, -15.550 -3.464 -2.000, -15.550 -3.864 -1.035, -15.550 11.250 -29.000, -15.550 11.250 10.000, -15.550 -11.250 -29.000, -15.550 -11.250 10.000, -15.550 -0.000 -9.500, -15.550 9.925 -29.000, -15.550 6.855 -29.000, -15.550 -6.855 -29.000, -15.550 -9.925 -29.000, -14.377 11.250 -29.000, -14.784 10.823 -29.000, -15.175 10.381 -29.000, -3.000 11.250 -9.500, 9.550 11.250 10.000, 9.550 11.250 -29.000, 9.500 11.250 -29.000, 8.377 11.250 -29.000, -2.500 11.250 -29.000, -11.828 11.250 -29.000, -17.027 -7.697 -29.000, -16.513 -4.678 -29.000, -18.113 -5.254 -29.000, -17.102 -2.372 -29.000, -18.777 -2.664 -29.000, -17.300 -0.000 -29.000, -19.000 -0.000 -29.000, -17.102 2.372 -29.000, -16.513 4.678 -29.000, -18.777 2.664 -29.000, -18.113 5.254 -29.000, -17.027 7.697 -29.000, 9.550 -11.250 10.000, 9.550 11.250 10.000, -16.425 0.000 -29.000, 9.550 -11.250 -29.000, -3.000 -11.250 -9.500, 9.500 -11.250 -29.000, 8.377 -11.250 -29.000, -14.377 -11.250 -29.000, -2.500 -11.250 -29.000, -11.828 -11.250 -29.000, -14.784 -10.823 -29.000, -15.175 -10.381 -29.000, -1.214 15.900 -31.000, -3.902 15.975 -31.000, -1.214 15.900 -29.000, -4.099 15.962 -29.000, -18.774 2.680 -31.000, -19.000 -0.000 -31.000, -18.102 5.283 -31.000, -17.004 7.738 -31.000, -15.511 9.974 -31.000, -13.664 11.928 -31.000, -12.172 13.110 -29.000, -11.515 13.546 -31.000, -9.668 14.544 -29.000, -9.126 14.781 -31.000, -6.947 15.505 -29.000, -6.565 15.598 -31.000, 9.550 -6.255 -4.988, 9.550 -4.988 -6.255, 9.550 3.471 7.208, 9.550 4.988 6.255, 9.550 6.255 -4.988, 9.550 4.988 -6.255, 9.550 -7.208 -3.471, 9.550 7.208 -3.471, 9.550 3.471 -7.208, 9.550 7.799 -1.780, 9.550 1.780 7.799, 9.550 0.000 8.000, 9.550 -8.000 -0.000, 9.550 -7.799 1.780, 9.550 -7.208 3.471, 9.550 -6.255 4.988, 9.550 -4.988 6.255, 9.550 -3.471 7.208, 9.550 -1.780 7.799, 9.550 -3.471 -7.208, 9.550 -7.799 -1.780, 9.550 0.000 -9.500, 9.550 6.255 4.988, 9.550 7.208 3.471, 9.550 1.780 -7.799, 9.550 7.799 1.780, 9.550 -0.000 -8.000, 9.550 8.000 -0.000, 9.550 -1.780 -7.799, 9.500 9.987 -29.000, 9.500 -9.987 -29.000, 9.525 0.000 -29.000, 9.500 14.200 -29.000, 7.147 12.371 -29.000, 5.808 13.358 -29.000, 4.373 14.200 -29.000, -1.312 14.200 -29.000, -2.500 14.200 -29.000, 2.939 12.725 -29.000, -3.000 14.300 -29.000, -3.000 15.900 -29.000, -6.145 13.950 -29.000, -9.137 12.916 -29.000, -3.000 14.200 -29.000, -7.164 12.775 -29.000, -3.000 -16.000 -31.000, -3.000 -16.000 -29.000, -5.688 -15.773 -29.000, -5.778 -15.757 -31.000, -8.472 -15.035 -31.000, -8.299 -15.097 -29.000, -11.000 -13.856 -31.000, -10.760 -13.992 -29.000, -13.000 -12.490 -29.000, -13.285 -12.257 -31.000, -13.475 -12.095 -29.000, -13.934 -11.681 -29.000, -15.257 -10.285 -31.000, -16.856 -8.000 -31.000, -18.035 -5.472 -31.000, -18.757 -2.778 -31.000, 9.500 -14.300 -29.000, 7.090 -12.417 -29.000, 5.685 -13.438 -29.000, 4.177 -14.300 -29.000, -2.500 -11.300 -29.000, -0.500 -11.300 -29.000, -0.500 -14.300 -29.000, 2.939 -12.775 -29.000, -13.000 -11.300 -29.000, -11.785 -11.283 -29.000, -11.764 -11.300 -29.000, -11.806 -11.267 -29.000, -7.164 -11.275 -29.000, -12.795 12.651 -49.775, -13.664 11.928 -31.000, -18.774 2.683 -55.500, -19.000 -0.000 -55.500, -7.606 15.323 -50.552, -8.803 14.911 -49.917, -10.114 14.331 -49.566, -17.000 7.746 -55.500, -18.101 5.289 -55.500, -16.978 7.786 -54.982, -18.102 5.283 -31.000, -16.131 9.142 -52.390, -16.677 8.303 -53.559, -17.004 7.738 -31.000, -16.911 7.905 -54.469, -5.773 15.758 -52.554, -6.621 15.585 -51.398, -6.565 15.598 -31.000, -14.068 11.555 -50.343, -15.210 10.341 -51.225, -3.633 15.987 -55.500, -4.588 15.921 -54.186, -5.182 15.851 -53.368, -2.825 15.999 -55.500, -2.018 15.970 -55.500, -1.214 15.900 -55.500, -11.351 13.648 -49.510, 9.500 15.900 -29.000, 9.500 15.900 -35.000, 1.882 15.900 -43.231, 1.223 15.900 -45.064, 2.940 15.900 -41.595, 8.000 15.900 -55.367, 8.000 15.900 -55.500, 6.115 15.900 -54.797, 4.343 15.900 -40.243, 6.017 15.900 -39.246, 1.000 15.900 -47.000, 13.551 15.900 -39.528, 15.146 15.900 -40.646, 4.411 15.900 -53.808, 2.981 15.900 -52.454, 1.900 15.900 -50.808, 1.228 15.900 -48.956, 7.875 15.900 -38.657, 9.817 15.900 -38.506, 11.743 15.900 -38.801, 10.550 -8.000 -0.000, 10.550 -7.799 1.780, 10.550 -7.208 3.471, 10.550 -6.255 4.988, 10.550 -4.988 6.255, 10.550 -3.471 7.208, 10.550 -1.780 7.799, 10.550 0.000 8.000, 10.550 1.780 7.799, 10.550 3.471 7.208, 10.550 4.988 6.255, 10.550 6.255 4.988, 10.550 7.208 3.471, 10.550 7.799 1.780, 10.550 8.000 -0.000, 10.550 7.799 -1.780, 10.550 7.208 -3.471, 10.550 6.255 -4.988, 10.550 4.988 -6.255, 10.550 3.471 -7.208, 10.550 1.780 -7.799, 10.550 -0.000 -8.000, 10.550 -1.780 -7.799, 10.550 -3.471 -7.208, 10.550 -4.988 -6.255, 10.550 -6.255 -4.988, 10.550 -7.208 -3.471, 10.550 -7.799 -1.780, 9.500 9.987 -31.000, 9.500 12.094 -30.000, 9.500 14.200 -31.000, 9.500 -9.987 -31.000, 9.500 -12.144 -30.000, 9.500 -14.300 -31.000, 9.500 15.000 -29.000, 2.568 15.000 -29.000, 6.034 14.600 -29.000, 3.180 14.758 -29.000, 3.782 14.491 -29.000, 9.500 14.200 -29.000, -2.500 14.291 -29.000, -2.103 14.272 -29.000, -2.500 15.000 -29.000, -1.707 14.241 -29.000, 0.936 14.600 -29.000, -2.833 14.299 -29.000, -2.667 14.296 -29.000, 1.333 15.402 -29.000, 0.070 15.703 -29.000, -2.750 14.250 -29.000, -2.500 -16.000 -31.000, 2.223 -16.000 -42.607, 3.668 -16.000 -40.816, 5.358 -16.000 -39.578, 9.500 -16.000 -35.000, 9.500 -16.000 -31.000, 6.424 -16.000 -54.924, 8.000 -16.000 -55.500, 8.000 -16.000 -55.367, 1.311 -16.000 -44.720, -2.500 -16.000 -29.000, 1.000 -16.000 -47.000, -3.000 -16.000 -55.500, 15.146 -16.000 -40.646, 13.421 -16.000 -39.458, 3.668 -16.000 -53.184, 2.223 -16.000 -51.393, 4.962 -16.000 -54.187, 1.311 -16.000 -49.280, 7.299 -16.000 -38.790, 9.374 -16.000 -38.501, 11.457 -16.000 -38.728, -13.741 -11.859 -50.162, -12.352 -12.982 -49.654, -13.285 -12.257 -31.000, -3.633 -15.987 -55.500, -3.422 -15.994 -55.500, -4.230 -15.953 -54.678, -4.826 -15.896 -53.858, -3.211 -15.999 -55.500, -15.889 -9.481 -52.021, -14.960 -10.628 -50.993, -17.000 -7.746 -55.500, -16.958 -7.821 -54.795, -18.101 -5.289 -55.500, -5.773 -15.758 -52.554, -5.778 -15.757 -31.000, -7.415 -15.379 -50.689, -6.456 -15.622 -51.582, -16.835 -8.037 -54.102, -16.548 -8.512 -53.215, -9.619 -14.567 -49.661, -8.535 -15.012 -50.030, -18.774 -2.683 -55.500, -10.939 -13.891 -49.500, -0.500 -20.300 -29.000, -0.500 -16.000 -29.000, -13.000 -20.300 -29.000, -6.750 -16.395 -29.000, 1.108 -15.464 -29.000, -0.500 -15.803 -29.000, 4.500 -15.150 -29.000, 2.672 -14.961 -29.000, 9.500 -16.000 -29.000, -6.750 -11.300 -15.750, -0.500 -11.300 -2.500, -13.000 -11.300 -2.500, -0.500 -15.800 -15.750, -0.500 -20.300 -2.500, -13.000 -15.800 -15.750, -13.000 -20.300 -2.500, -5.183 14.132 -53.367, -5.773 14.029 -52.554, -3.633 14.286 -55.500, -4.703 15.008 -54.027, -4.589 14.211 -54.184, -3.000 15.900 -55.500, -3.211 14.298 -55.500, -3.422 14.294 -55.500, -3.000 14.300 -55.500, -10.197 12.357 -49.554, -11.497 11.502 -49.521, -8.829 13.058 -49.907, -16.998 2.925 -55.334, -17.000 2.914 -55.500, -7.607 13.537 -50.551, -16.991 2.957 -55.169, -16.964 3.082 -54.844, -6.619 13.834 -51.400, -16.884 3.422 -54.328, -16.664 4.218 -53.520, -16.073 5.797 -52.295, -15.159 7.526 -51.176, -14.059 9.065 -50.339, -12.837 10.379 -49.788, -17.000 -2.914 -55.500, -17.166 -1.950 -55.500, -17.267 -0.977 -55.500, -17.166 1.950 -55.500, -17.267 0.977 -55.500, -18.000 0.000 -55.500, -17.300 -0.000 -55.500, 4.373 14.200 -55.500, 8.000 14.200 -55.500, 0.713 15.563 -55.500, 3.393 15.050 -55.500, 2.585 14.994 -55.500, 8.000 14.200 -55.367, 4.143 15.450 -29.000, 11.293 15.900 -43.424, 16.095 15.900 -41.638, 13.267 15.900 -45.656, 12.808 15.900 -44.751, 16.016 15.900 -41.542, 5.675 15.900 -48.169, 5.500 15.900 -47.000, 15.936 15.900 -41.447, 15.854 15.900 -41.354, 10.335 15.900 -43.088, 9.324 15.900 -43.004, 8.000 15.900 -50.708, 6.982 15.900 -50.108, 12.135 15.900 -43.991, 8.323 15.900 -43.177, 6.183 15.900 -49.236, 5.629 15.900 -45.993, 18.000 15.900 -47.000, 17.998 15.900 -47.167, 15.000 15.900 -47.500, 17.993 15.900 -47.333, 7.399 15.900 -43.596, 17.985 15.900 -47.500, 17.847 15.900 -45.394, 17.932 15.900 -45.926, 17.983 15.900 -46.462, 6.007 15.900 -45.051, 6.610 15.900 -44.235, 15.000 15.900 -49.500, 13.151 15.900 -48.634, 12.622 15.900 -49.500, 13.445 15.900 -47.662, 13.484 15.900 -46.648, 9.500 15.000 -31.000, 9.500 15.050 -32.000, 9.500 14.200 -35.000, 9.500 14.200 -31.000, 15.146 14.200 -40.646, 10.550 0.637 -0.771, 10.550 0.809 -0.588, 10.550 0.426 -0.905, 10.550 0.930 -0.368, 10.550 0.187 -0.982, 10.550 0.992 -0.125, 10.550 -0.063 -0.998, 10.550 0.992 0.125, 10.550 -0.309 -0.951, 10.550 0.930 0.368, 10.550 -0.536 -0.844, 10.550 0.809 0.588, 10.550 -0.729 -0.685, 10.550 0.637 0.771, 10.550 0.426 0.905, 10.550 -0.876 -0.482, 10.550 -0.969 -0.249, 10.550 0.187 0.982, 10.550 -1.000 -0.000, 10.550 -0.063 0.998, 10.550 -0.309 0.951, 10.550 -0.969 0.249, 10.550 -0.536 0.844, 10.550 -0.876 0.482, 10.550 -0.729 0.685, 9.500 9.987 -31.000, 7.993 11.626 -31.000, 6.273 13.039 -31.000, 4.373 14.200 -31.000, -1.312 14.200 -31.000, 3.500 -0.050 -31.000, 9.500 -9.987 -31.000, -2.500 14.200 -31.000, -2.500 -14.300 -31.000, 4.177 -14.300 -31.000, 7.939 -11.676 -31.000, 6.153 -13.124 -31.000, 9.500 -16.000 -29.000, 9.500 -14.300 -31.000, 9.500 -14.300 -29.000, 9.500 -16.000 -31.000, 6.853 -16.000 -51.242, 6.424 -16.000 -54.924, 8.000 -16.000 -51.770, 16.095 -16.000 -41.638, 13.830 -16.000 -44.500, 17.847 -16.000 -45.394, 4.500 -16.000 -47.000, 4.670 -16.000 -45.706, 4.659 -16.000 -48.253, 13.036 -16.000 -43.464, 15.936 -16.000 -41.447, 15.854 -16.000 -41.354, 8.206 -16.000 -42.170, 9.500 -16.000 -42.000, 16.016 -16.000 -41.542, 5.875 -16.000 -50.443, 7.299 -16.000 -38.790, 5.170 -16.000 -44.500, 12.000 -16.000 -42.670, 5.358 -16.000 -39.578, 7.000 -16.000 -42.670, 14.330 -16.000 -48.294, 13.830 -16.000 -49.500, 15.000 -16.000 -49.500, 5.964 -16.000 -43.464, 5.128 -16.000 -49.425, 15.000 -16.000 -47.500, 17.998 -16.000 -47.167, 18.000 -16.000 -47.000, 17.993 -16.000 -47.333, 17.985 -16.000 -47.500, 17.983 -16.000 -46.462, 17.932 -16.000 -45.926, 10.794 -16.000 -42.170, 14.500 -16.000 -47.000, 14.330 -16.000 -45.706, 8.000 -16.000 -55.367, 8.000 -14.300 -55.500, 8.000 -14.300 -55.367, 4.177 -14.300 -55.500, 1.883 -15.237 -55.500, -0.529 -15.808 -55.500, 9.500 -14.300 -35.000, 9.500 -16.000 -35.000, 15.146 -14.300 -40.646, 3.500 -16.000 -30.000, -4.231 -14.247 -54.677, -3.633 -14.286 -55.500, -5.773 -14.029 -52.554, -4.703 -15.008 -54.027, -4.827 -14.183 -53.857, -3.316 -15.143 -55.500, -3.000 -14.300 -55.500, -3.211 -14.298 -55.500, -3.422 -14.294 -55.500, -8.126 -13.350 -50.233, -7.157 -13.683 -50.893, -16.992 -2.953 -55.186, -6.359 -13.900 -51.697, -16.967 -3.066 -54.875, -16.911 -3.312 -54.471, -16.779 -3.825 -53.886, -16.478 -4.779 -53.052, -15.784 -6.409 -51.878, -14.763 -8.131 -50.827, -13.519 -9.687 -50.055, -12.148 -10.991 -49.611, -10.766 -12.008 -49.505, -9.362 -12.807 -49.728, -13.000 -20.300 -29.000, -0.500 -20.300 -29.000, -13.000 -20.300 -2.500, -0.500 -20.300 -2.500, -0.500 -11.300 -2.500, -13.000 -11.300 -2.500, -12.192 10.954 -31.000, -5.483 14.083 -31.000, -13.954 9.192 -31.000, -7.891 13.438 -31.000, -3.000 14.300 -31.000, -10.150 12.384 -31.000, -17.300 -0.000 -31.000, -17.083 2.483 -31.000, -16.438 4.891 -31.000, -15.384 7.150 -31.000, -1.312 14.200 -55.500, -1.873 14.256 -55.500, -2.436 14.289 -55.500, 0.686 15.050 -55.500, -17.083 -2.483 -31.000, -12.192 -10.954 -31.000, -10.150 -12.384 -31.000, -13.954 -9.192 -31.000, -15.384 -7.150 -31.000, -3.000 -14.300 -31.000, -5.483 -14.083 -31.000, -7.891 -13.438 -31.000, -16.438 -4.891 -31.000, 6.713 14.200 -55.030, 5.494 14.200 -54.497, 4.373 14.200 -53.780, 17.985 14.200 -47.500, 17.620 14.200 -49.513, 17.620 15.900 -49.513, 16.784 15.900 -51.381, 16.784 14.200 -51.381, 15.526 15.900 -52.995, 15.526 14.200 -52.995, 13.918 15.900 -54.262, 13.918 14.200 -54.262, 12.055 15.900 -55.107, 12.055 14.200 -55.107, 10.043 15.900 -55.483, 10.043 14.200 -55.483, 18.000 15.900 -47.500, 12.993 15.900 -51.491, 10.997 15.900 -50.709, 10.009 15.900 -50.967, 8.988 15.900 -50.967, 11.888 15.900 -50.209, 6.982 14.200 -50.108, 8.000 14.200 -50.708, 12.808 14.200 -44.751, 12.135 14.200 -43.991, 6.183 14.200 -49.236, 13.267 14.200 -45.656, 5.629 14.200 -45.993, 5.500 14.200 -47.000, 5.675 14.200 -48.169, 6.007 14.200 -45.051, 13.484 14.200 -46.648, 13.445 14.200 -47.662, 6.610 14.200 -44.235, 13.151 14.200 -48.634, 7.399 14.200 -43.596, 12.622 14.200 -49.500, 8.323 14.200 -43.177, 11.888 14.200 -50.209, 9.324 14.200 -43.004, 10.997 14.200 -50.709, 10.335 14.200 -43.088, 10.009 14.200 -50.967, 11.293 14.200 -43.424, 8.988 14.200 -50.967, 15.391 15.900 -40.873, 15.627 15.900 -41.109, 16.058 15.900 -41.558, 17.472 15.900 -44.052, 16.883 15.900 -42.788, 18.000 15.900 -45.723, 6.046 14.200 -39.234, 7.899 14.200 -38.652, 4.373 14.200 -40.220, 13.557 14.200 -39.530, 9.760 14.200 -35.823, 9.836 14.200 -38.507, 11.755 14.200 -38.805, 15.391 14.200 -40.873, 15.627 14.200 -41.109, 15.854 14.200 -41.354, 10.550 -0.004 0.000, 2.958 14.200 -52.427, 1.890 14.200 -50.786, 1.225 14.200 -48.945, 1.000 14.200 -47.000, 1.225 14.200 -45.055, 1.890 14.200 -43.214, 2.958 14.200 -41.573, -2.156 14.200 -43.250, -3.000 14.200 -55.500, -3.000 14.200 -31.000, -9.900 -0.000 -31.000, 4.177 -14.300 -53.627, 2.841 -14.300 -52.282, 1.835 -14.300 -50.675, 1.211 -14.300 -48.884, 1.000 -14.300 -47.000, 4.177 -14.300 -40.373, 2.841 -14.300 -41.718, 1.211 -14.300 -45.116, 0.589 -14.300 -43.250, 1.835 -14.300 -43.325, 7.737 -14.300 -38.685, 5.858 -14.300 -39.320, 13.520 -14.300 -39.511, 9.712 -14.300 -38.503, 9.662 -14.300 -35.823, 11.675 -14.300 -38.783, 18.000 -16.000 -47.500, 16.784 -16.000 -51.381, 17.620 -16.000 -49.513, 15.526 -16.000 -52.995, 13.005 -16.000 -50.566, 10.043 -16.000 -55.483, 12.055 -16.000 -55.107, 10.669 -16.000 -51.861, 11.925 -16.000 -51.372, 9.328 -16.000 -51.997, 12.993 -16.000 -51.491, 13.918 -16.000 -54.262, 6.853 -14.300 -51.242, 5.875 -14.300 -50.443, 13.036 -14.300 -43.464, 13.830 -14.300 -44.500, 5.128 -14.300 -49.425, 14.330 -14.300 -45.706, 4.500 -14.300 -47.000, 4.670 -14.300 -45.706, 4.659 -14.300 -48.253, 14.500 -14.300 -47.000, 5.170 -14.300 -44.500, 14.330 -14.300 -48.294, 5.964 -14.300 -43.464, 13.830 -14.300 -49.500, 7.000 -14.300 -42.670, 13.005 -14.300 -50.566, 8.206 -14.300 -42.170, 11.925 -14.300 -51.372, 9.500 -14.300 -42.000, 10.669 -14.300 -51.861, 10.794 -14.300 -42.170, 9.328 -14.300 -51.997, 8.000 -14.300 -51.770, 12.000 -14.300 -42.670, 15.391 -16.000 -40.873, 15.627 -16.000 -41.109, 16.058 -16.000 -41.558, 16.883 -16.000 -42.788, 17.472 -16.000 -44.052, 18.000 -16.000 -45.723, 6.635 -14.300 -55.002, 5.348 -14.300 -54.417, 17.985 -14.300 -47.500, 17.620 -14.300 -49.513, 16.784 -14.300 -51.381, 15.526 -14.300 -52.995, 13.918 -14.300 -54.262, 12.055 -14.300 -55.107, 10.043 -14.300 -55.483, 0.589 -15.150 -55.500, 15.391 -14.300 -40.873, 15.627 -14.300 -41.109, 15.854 -14.300 -41.354, -2.156 14.250 -55.500, 16.095 14.200 -41.638, 17.847 14.200 -45.394, 16.016 14.200 -41.542, 15.936 14.200 -41.447, 15.000 14.200 -47.500, 17.998 14.200 -47.167, 18.000 14.200 -47.000, 17.993 14.200 -47.333, 17.932 14.200 -45.926, 17.983 14.200 -46.462, 15.000 14.200 -49.500, 18.000 14.200 -47.500, 12.993 14.200 -51.491, 16.058 14.200 -41.558, 16.883 14.200 -42.788, 17.472 14.200 -44.052, 18.000 14.200 -45.723, 2.686 14.200 -47.000, 2.589 -14.300 -47.000, 17.847 -14.300 -45.394, 16.095 -14.300 -41.638, 15.936 -14.300 -41.447, 16.016 -14.300 -41.542, 15.000 -14.300 -49.500, 18.000 -14.300 -47.000, 17.998 -14.300 -47.167, 15.000 -14.300 -47.500, 17.993 -14.300 -47.333, 17.932 -14.300 -45.926, 17.983 -14.300 -46.462, 18.000 -14.300 -47.500, 12.993 -14.300 -51.491, 16.058 -14.300 -41.558, 16.883 -14.300 -42.788, 17.472 -14.300 -44.052, 18.000 -14.300 -45.723 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 0, 2, 3, -1, 0, 3, 4, -1, 0, 4, 5, -1, 0, 5, 6, -1, 0, 6, 7, -1, 0, 7, 8, -1, 0, 8, 9, -1, 0, 9, 10, -1, 0, 10, 11, -1, 0, 11, 12, -1, 0, 12, 13, -1, 0, 13, 14, -1, 0, 14, 15, -1, 0, 15, 16, -1, 0, 16, 17, -1, 0, 17, 18, -1, 0, 18, 19, -1, 0, 19, 20, -1, 0, 20, 21, -1, 0, 21, 22, -1, 0, 22, 23, -1, 0, 23, 24, -1, 0, 24, 25, -1, 0, 25, 1, -1, 26, 27, 2, -1, 2, 27, 28, -1, 2, 28, 3, -1, 3, 28, 29, -1, 3, 29, 4, -1, 4, 29, 30, -1, 4, 30, 5, -1, 5, 30, 31, -1, 5, 31, 32, -1, 32, 31, 33, -1, 32, 33, 7, -1, 7, 33, 34, -1, 7, 34, 35, -1, 35, 34, 36, -1, 35, 36, 9, -1, 9, 36, 37, -1, 9, 37, 38, -1, 38, 37, 39, -1, 38, 39, 11, -1, 11, 39, 40, -1, 11, 40, 41, -1, 41, 40, 42, -1, 41, 42, 13, -1, 13, 42, 43, -1, 13, 43, 14, -1, 14, 43, 44, -1, 14, 44, 45, -1, 45, 44, 46, -1, 45, 46, 16, -1, 16, 46, 47, -1, 16, 47, 48, -1, 48, 47, 49, -1, 48, 49, 18, -1, 18, 49, 50, -1, 18, 50, 51, -1, 51, 50, 52, -1, 51, 52, 53, -1, 53, 52, 54, -1, 53, 54, 55, -1, 55, 54, 56, -1, 55, 56, 57, -1, 57, 56, 58, -1, 57, 58, 23, -1, 23, 58, 59, -1, 23, 59, 24, -1, 24, 59, 60, -1, 24, 60, 25, -1, 26, 25, 27, -1, 25, 60, 27, -1, 61, 44, 62, -1, 62, 40, 39, -1, 62, 39, 37, -1, 62, 37, 36, -1, 59, 63, 60, -1, 62, 36, 34, -1, 61, 46, 44, -1, 27, 60, 64, -1, 28, 27, 64, -1, 29, 28, 64, -1, 30, 29, 64, -1, 31, 30, 64, -1, 33, 31, 64, -1, 34, 33, 64, -1, 62, 34, 64, -1, 60, 63, 64, -1, 54, 52, 65, -1, 56, 54, 65, -1, 58, 56, 65, -1, 59, 58, 65, -1, 47, 46, 65, -1, 49, 47, 65, -1, 50, 49, 65, -1, 52, 50, 65, -1, 61, 66, 65, -1, 66, 67, 65, -1, 67, 68, 65, -1, 68, 69, 65, -1, 69, 63, 65, -1, 46, 61, 65, -1, 63, 59, 65, -1, 42, 40, 62, -1, 43, 42, 62, -1, 44, 43, 62, -1, 61, 70, 71, -1, 72, 61, 71, -1, 66, 61, 72, -1, 73, 74, 75, -1, 73, 62, 74, -1, 73, 75, 76, -1, 73, 61, 62, -1, 73, 76, 77, -1, 73, 70, 61, -1, 73, 77, 78, -1, 73, 79, 70, -1, 73, 78, 79, -1, 69, 68, 80, -1, 68, 81, 82, -1, 80, 68, 82, -1, 81, 83, 84, -1, 82, 81, 84, -1, 83, 85, 86, -1, 84, 83, 86, -1, 86, 85, 87, -1, 87, 88, 89, -1, 86, 87, 89, -1, 88, 67, 90, -1, 89, 88, 90, -1, 90, 67, 91, -1, 91, 67, 66, -1, 92, 93, 62, -1, 92, 62, 64, -1, 68, 67, 94, -1, 87, 85, 94, -1, 88, 87, 94, -1, 67, 88, 94, -1, 81, 68, 94, -1, 83, 81, 94, -1, 85, 83, 94, -1, 95, 92, 96, -1, 92, 64, 96, -1, 97, 95, 96, -1, 64, 63, 96, -1, 98, 97, 96, -1, 63, 99, 96, -1, 100, 98, 96, -1, 99, 101, 96, -1, 101, 100, 96, -1, 63, 102, 99, -1, 63, 103, 102, -1, 63, 69, 103, -1, 104, 105, 106, -1, 105, 107, 106, -1, 108, 109, 89, -1, 109, 86, 89, -1, 110, 108, 90, -1, 108, 89, 90, -1, 111, 110, 91, -1, 110, 90, 91, -1, 112, 111, 66, -1, 111, 91, 66, -1, 112, 66, 72, -1, 112, 72, 71, -1, 71, 70, 113, -1, 112, 71, 113, -1, 113, 70, 114, -1, 113, 114, 115, -1, 115, 114, 116, -1, 115, 116, 117, -1, 117, 116, 118, -1, 117, 118, 119, -1, 105, 119, 107, -1, 119, 118, 107, -1, 120, 95, 121, -1, 122, 123, 74, -1, 124, 125, 75, -1, 126, 95, 120, -1, 127, 124, 75, -1, 128, 75, 125, -1, 129, 127, 75, -1, 130, 122, 74, -1, 131, 130, 74, -1, 92, 132, 133, -1, 92, 133, 134, -1, 92, 134, 135, -1, 92, 135, 136, -1, 92, 136, 137, -1, 92, 137, 138, -1, 92, 138, 131, -1, 92, 131, 74, -1, 92, 95, 132, -1, 121, 95, 139, -1, 132, 95, 140, -1, 140, 95, 126, -1, 141, 95, 75, -1, 74, 142, 143, -1, 141, 128, 144, -1, 74, 143, 145, -1, 141, 144, 146, -1, 74, 145, 147, -1, 141, 146, 148, -1, 74, 147, 129, -1, 141, 148, 139, -1, 141, 75, 128, -1, 141, 139, 95, -1, 74, 129, 75, -1, 123, 142, 74, -1, 149, 76, 75, -1, 97, 150, 95, -1, 151, 150, 149, -1, 151, 75, 95, -1, 151, 149, 75, -1, 151, 95, 150, -1, 77, 76, 152, -1, 77, 152, 153, -1, 153, 152, 154, -1, 154, 152, 155, -1, 78, 156, 157, -1, 154, 155, 158, -1, 153, 154, 158, -1, 77, 153, 158, -1, 155, 156, 158, -1, 78, 77, 158, -1, 156, 78, 158, -1, 159, 160, 107, -1, 159, 107, 161, -1, 162, 161, 118, -1, 161, 107, 118, -1, 162, 118, 116, -1, 79, 162, 114, -1, 162, 116, 114, -1, 79, 114, 70, -1, 107, 160, 106, -1, 78, 157, 163, -1, 163, 159, 161, -1, 78, 163, 161, -1, 162, 79, 164, -1, 161, 162, 164, -1, 79, 78, 164, -1, 78, 161, 164, -1, 109, 84, 86, -1, 165, 166, 167, -1, 168, 165, 167, -1, 169, 168, 170, -1, 168, 167, 170, -1, 171, 169, 172, -1, 169, 170, 172, -1, 171, 172, 173, -1, 171, 173, 174, -1, 174, 173, 175, -1, 174, 175, 176, -1, 174, 176, 99, -1, 99, 102, 177, -1, 174, 99, 177, -1, 177, 102, 103, -1, 177, 103, 69, -1, 177, 69, 178, -1, 178, 69, 80, -1, 178, 80, 179, -1, 179, 80, 82, -1, 179, 82, 180, -1, 180, 82, 84, -1, 180, 84, 109, -1, 181, 97, 98, -1, 181, 98, 182, -1, 181, 182, 183, -1, 181, 183, 184, -1, 185, 186, 100, -1, 186, 98, 100, -1, 186, 187, 188, -1, 187, 184, 188, -1, 182, 98, 188, -1, 183, 182, 188, -1, 184, 183, 188, -1, 98, 186, 188, -1, 175, 173, 189, -1, 175, 189, 176, -1, 176, 189, 99, -1, 190, 189, 191, -1, 192, 189, 190, -1, 101, 189, 192, -1, 99, 189, 101, -1, 185, 100, 193, -1, 100, 101, 193, -1, 190, 191, 193, -1, 192, 190, 193, -1, 101, 192, 193, -1, 191, 185, 193, -1, 194, 195, 115, -1, 196, 197, 108, -1, 197, 109, 108, -1, 198, 199, 117, -1, 199, 200, 117, -1, 200, 115, 117, -1, 201, 202, 203, -1, 202, 196, 204, -1, 196, 108, 204, -1, 203, 202, 204, -1, 205, 206, 207, -1, 206, 208, 207, -1, 208, 203, 207, -1, 203, 204, 207, -1, 209, 210, 211, -1, 210, 198, 211, -1, 198, 117, 211, -1, 212, 213, 112, -1, 213, 205, 112, -1, 205, 207, 112, -1, 214, 215, 105, -1, 215, 216, 105, -1, 216, 209, 105, -1, 217, 214, 105, -1, 194, 212, 195, -1, 209, 211, 105, -1, 212, 112, 195, -1, 218, 217, 104, -1, 219, 218, 104, -1, 217, 105, 104, -1, 200, 220, 115, -1, 220, 194, 115, -1, 221, 222, 104, -1, 223, 224, 104, -1, 225, 223, 104, -1, 226, 227, 228, -1, 229, 225, 104, -1, 230, 229, 104, -1, 222, 230, 104, -1, 221, 104, 106, -1, 231, 219, 104, -1, 224, 231, 104, -1, 232, 222, 233, -1, 234, 228, 219, -1, 235, 234, 219, -1, 228, 227, 219, -1, 235, 219, 236, -1, 236, 219, 237, -1, 237, 219, 231, -1, 238, 230, 222, -1, 239, 238, 222, -1, 240, 239, 222, -1, 232, 240, 222, -1, 132, 241, 133, -1, 133, 241, 242, -1, 134, 133, 243, -1, 133, 242, 243, -1, 135, 134, 244, -1, 134, 243, 244, -1, 136, 135, 245, -1, 135, 244, 245, -1, 136, 245, 137, -1, 137, 245, 246, -1, 138, 137, 247, -1, 137, 246, 247, -1, 131, 138, 248, -1, 138, 247, 248, -1, 130, 131, 249, -1, 131, 248, 249, -1, 122, 130, 250, -1, 130, 249, 250, -1, 122, 250, 123, -1, 123, 250, 251, -1, 123, 251, 252, -1, 142, 123, 252, -1, 142, 252, 253, -1, 143, 142, 253, -1, 143, 253, 145, -1, 145, 253, 254, -1, 147, 145, 255, -1, 145, 254, 255, -1, 129, 147, 256, -1, 147, 255, 256, -1, 127, 129, 257, -1, 129, 256, 257, -1, 124, 127, 258, -1, 127, 257, 258, -1, 125, 124, 259, -1, 124, 258, 259, -1, 128, 125, 260, -1, 125, 259, 260, -1, 128, 260, 144, -1, 144, 260, 261, -1, 146, 144, 262, -1, 144, 261, 262, -1, 148, 146, 263, -1, 146, 262, 263, -1, 139, 148, 264, -1, 148, 263, 264, -1, 121, 139, 265, -1, 139, 264, 265, -1, 120, 121, 266, -1, 121, 265, 266, -1, 126, 120, 267, -1, 120, 266, 267, -1, 140, 126, 268, -1, 126, 267, 268, -1, 140, 268, 132, -1, 132, 268, 241, -1, 149, 269, 76, -1, 270, 152, 76, -1, 270, 269, 271, -1, 270, 271, 152, -1, 270, 76, 269, -1, 150, 272, 269, -1, 150, 269, 149, -1, 97, 272, 150, -1, 273, 97, 181, -1, 273, 181, 274, -1, 273, 274, 272, -1, 273, 272, 97, -1, 275, 276, 277, -1, 276, 278, 277, -1, 278, 279, 277, -1, 279, 155, 277, -1, 155, 280, 277, -1, 280, 275, 277, -1, 281, 282, 283, -1, 282, 284, 283, -1, 284, 156, 283, -1, 156, 155, 285, -1, 278, 276, 285, -1, 279, 278, 285, -1, 155, 279, 285, -1, 276, 283, 285, -1, 283, 156, 285, -1, 284, 282, 157, -1, 156, 284, 157, -1, 157, 282, 281, -1, 159, 286, 283, -1, 286, 287, 283, -1, 287, 281, 283, -1, 159, 283, 160, -1, 283, 106, 160, -1, 288, 283, 276, -1, 289, 283, 288, -1, 106, 283, 289, -1, 157, 281, 287, -1, 163, 286, 159, -1, 163, 157, 290, -1, 287, 286, 290, -1, 157, 287, 290, -1, 286, 163, 290, -1, 291, 292, 293, -1, 291, 293, 294, -1, 291, 295, 296, -1, 297, 298, 299, -1, 291, 294, 295, -1, 165, 300, 292, -1, 165, 292, 291, -1, 166, 291, 301, -1, 166, 165, 291, -1, 302, 300, 303, -1, 303, 300, 165, -1, 304, 295, 305, -1, 303, 306, 307, -1, 303, 297, 308, -1, 303, 308, 306, -1, 303, 298, 297, -1, 309, 303, 307, -1, 302, 303, 309, -1, 295, 294, 310, -1, 295, 310, 311, -1, 295, 311, 312, -1, 295, 312, 305, -1, 313, 314, 315, -1, 314, 171, 315, -1, 316, 317, 318, -1, 319, 318, 165, -1, 320, 303, 165, -1, 317, 320, 165, -1, 318, 317, 165, -1, 321, 322, 177, -1, 322, 313, 177, -1, 313, 315, 177, -1, 323, 324, 325, -1, 326, 319, 327, -1, 328, 329, 327, -1, 324, 330, 178, -1, 329, 326, 327, -1, 330, 331, 178, -1, 319, 165, 327, -1, 331, 321, 178, -1, 321, 177, 178, -1, 325, 324, 179, -1, 324, 178, 179, -1, 332, 333, 169, -1, 333, 328, 169, -1, 334, 325, 180, -1, 328, 327, 169, -1, 325, 179, 180, -1, 197, 334, 109, -1, 314, 335, 171, -1, 334, 180, 109, -1, 335, 332, 171, -1, 332, 169, 171, -1, 336, 301, 337, -1, 336, 166, 301, -1, 338, 172, 170, -1, 338, 173, 172, -1, 339, 167, 166, -1, 339, 170, 167, -1, 339, 336, 338, -1, 339, 166, 336, -1, 339, 338, 170, -1, 337, 340, 341, -1, 181, 184, 342, -1, 184, 343, 342, -1, 343, 340, 342, -1, 337, 344, 342, -1, 344, 181, 342, -1, 340, 337, 342, -1, 345, 346, 186, -1, 345, 347, 346, -1, 345, 186, 185, -1, 345, 189, 347, -1, 345, 185, 191, -1, 345, 191, 189, -1, 348, 187, 186, -1, 348, 186, 346, -1, 348, 341, 187, -1, 348, 346, 349, -1, 348, 337, 341, -1, 348, 349, 336, -1, 348, 336, 337, -1, 340, 343, 187, -1, 341, 340, 187, -1, 343, 184, 187, -1, 189, 173, 350, -1, 347, 189, 350, -1, 173, 338, 350, -1, 351, 347, 350, -1, 338, 351, 350, -1, 209, 216, 352, -1, 209, 352, 353, -1, 214, 354, 355, -1, 354, 356, 355, -1, 356, 352, 355, -1, 215, 214, 355, -1, 216, 215, 355, -1, 352, 216, 355, -1, 357, 358, 359, -1, 357, 360, 358, -1, 357, 217, 218, -1, 214, 359, 354, -1, 214, 217, 357, -1, 214, 357, 359, -1, 219, 357, 218, -1, 361, 362, 220, -1, 363, 200, 199, -1, 364, 365, 201, -1, 363, 361, 200, -1, 366, 199, 198, -1, 367, 201, 203, -1, 367, 364, 201, -1, 366, 363, 199, -1, 368, 367, 203, -1, 369, 198, 210, -1, 369, 366, 198, -1, 208, 368, 203, -1, 209, 369, 210, -1, 370, 368, 208, -1, 353, 369, 209, -1, 206, 370, 208, -1, 371, 370, 206, -1, 205, 371, 206, -1, 372, 371, 205, -1, 213, 372, 205, -1, 373, 372, 213, -1, 212, 373, 213, -1, 374, 373, 212, -1, 375, 212, 194, -1, 375, 374, 212, -1, 362, 375, 194, -1, 220, 362, 194, -1, 361, 220, 200, -1, 376, 323, 325, -1, 334, 377, 376, -1, 334, 376, 325, -1, 378, 377, 334, -1, 197, 378, 334, -1, 196, 379, 380, -1, 196, 365, 379, -1, 196, 380, 197, -1, 202, 365, 196, -1, 201, 365, 202, -1, 381, 382, 378, -1, 381, 380, 382, -1, 381, 197, 380, -1, 381, 378, 197, -1, 383, 227, 384, -1, 219, 227, 385, -1, 386, 387, 385, -1, 386, 383, 387, -1, 386, 385, 227, -1, 386, 227, 383, -1, 388, 227, 226, -1, 388, 384, 227, -1, 221, 106, 389, -1, 106, 289, 389, -1, 289, 288, 389, -1, 288, 276, 389, -1, 276, 275, 389, -1, 275, 221, 389, -1, 232, 390, 240, -1, 391, 392, 393, -1, 391, 393, 394, -1, 395, 236, 237, -1, 396, 395, 237, -1, 394, 393, 397, -1, 397, 393, 398, -1, 399, 400, 239, -1, 240, 399, 239, -1, 401, 234, 402, -1, 398, 393, 403, -1, 396, 237, 231, -1, 400, 404, 238, -1, 398, 403, 233, -1, 239, 400, 238, -1, 402, 234, 235, -1, 402, 235, 405, -1, 406, 396, 224, -1, 396, 231, 224, -1, 407, 408, 409, -1, 408, 410, 409, -1, 404, 411, 230, -1, 410, 412, 409, -1, 413, 414, 409, -1, 238, 404, 230, -1, 414, 415, 409, -1, 415, 407, 409, -1, 416, 406, 223, -1, 406, 224, 223, -1, 233, 403, 232, -1, 411, 417, 229, -1, 403, 390, 232, -1, 409, 418, 419, -1, 418, 420, 419, -1, 230, 411, 229, -1, 405, 235, 236, -1, 417, 416, 225, -1, 416, 223, 225, -1, 395, 405, 236, -1, 229, 417, 225, -1, 409, 419, 421, -1, 409, 421, 422, -1, 413, 409, 392, -1, 409, 422, 392, -1, 226, 228, 401, -1, 401, 228, 234, -1, 413, 392, 391, -1, 390, 399, 240, -1, 275, 423, 221, -1, 424, 222, 221, -1, 424, 425, 222, -1, 424, 426, 425, -1, 424, 423, 426, -1, 424, 221, 423, -1, 222, 425, 233, -1, 233, 425, 427, -1, 259, 258, 428, -1, 258, 257, 429, -1, 428, 258, 429, -1, 260, 259, 430, -1, 261, 260, 430, -1, 259, 428, 430, -1, 257, 256, 431, -1, 429, 257, 431, -1, 262, 261, 432, -1, 261, 430, 432, -1, 256, 255, 433, -1, 431, 256, 433, -1, 263, 262, 434, -1, 262, 432, 434, -1, 255, 254, 435, -1, 433, 255, 435, -1, 264, 263, 436, -1, 263, 434, 436, -1, 254, 253, 437, -1, 435, 254, 437, -1, 264, 436, 438, -1, 265, 264, 438, -1, 253, 252, 439, -1, 437, 253, 439, -1, 265, 438, 440, -1, 439, 252, 441, -1, 442, 441, 251, -1, 441, 252, 251, -1, 265, 440, 266, -1, 440, 443, 266, -1, 442, 251, 250, -1, 266, 443, 267, -1, 443, 444, 267, -1, 445, 442, 249, -1, 442, 250, 249, -1, 267, 444, 268, -1, 444, 446, 268, -1, 447, 445, 248, -1, 445, 249, 248, -1, 268, 446, 241, -1, 448, 447, 247, -1, 447, 248, 247, -1, 446, 449, 242, -1, 241, 446, 242, -1, 450, 448, 246, -1, 448, 247, 246, -1, 449, 451, 243, -1, 242, 449, 243, -1, 452, 450, 245, -1, 450, 246, 245, -1, 451, 452, 244, -1, 452, 245, 244, -1, 243, 451, 244, -1, 426, 453, 454, -1, 455, 426, 454, -1, 456, 426, 455, -1, 280, 423, 275, -1, 280, 426, 423, -1, 457, 456, 455, -1, 458, 453, 459, -1, 458, 455, 454, -1, 458, 454, 453, -1, 458, 460, 457, -1, 458, 461, 460, -1, 458, 462, 461, -1, 458, 459, 463, -1, 458, 463, 464, -1, 458, 464, 462, -1, 458, 457, 455, -1, 465, 466, 467, -1, 465, 468, 466, -1, 459, 466, 463, -1, 464, 463, 466, -1, 462, 464, 466, -1, 469, 308, 470, -1, 469, 470, 471, -1, 472, 473, 474, -1, 302, 475, 476, -1, 302, 477, 475, -1, 302, 309, 477, -1, 306, 308, 469, -1, 478, 479, 480, -1, 311, 481, 482, -1, 478, 483, 479, -1, 311, 482, 312, -1, 478, 472, 483, -1, 478, 473, 472, -1, 484, 306, 469, -1, 485, 481, 311, -1, 300, 476, 486, -1, 304, 478, 480, -1, 300, 302, 476, -1, 487, 478, 304, -1, 488, 489, 481, -1, 490, 491, 492, -1, 488, 481, 485, -1, 292, 486, 493, -1, 292, 300, 486, -1, 307, 484, 494, -1, 307, 306, 484, -1, 495, 496, 497, -1, 495, 498, 496, -1, 293, 493, 489, -1, 495, 499, 498, -1, 293, 292, 493, -1, 293, 489, 488, -1, 495, 500, 501, -1, 495, 497, 500, -1, 495, 490, 492, -1, 305, 487, 304, -1, 305, 502, 487, -1, 503, 490, 495, -1, 504, 501, 474, -1, 504, 495, 501, -1, 309, 307, 494, -1, 504, 503, 495, -1, 309, 494, 477, -1, 471, 470, 505, -1, 473, 504, 474, -1, 312, 482, 502, -1, 312, 502, 305, -1, 505, 506, 507, -1, 505, 298, 506, -1, 508, 506, 298, -1, 509, 508, 298, -1, 510, 509, 298, -1, 303, 510, 298, -1, 511, 512, 513, -1, 513, 512, 304, -1, 466, 512, 511, -1, 468, 512, 466, -1, 301, 291, 337, -1, 514, 291, 296, -1, 514, 296, 344, -1, 514, 344, 337, -1, 514, 337, 291, -1, 515, 516, 318, -1, 516, 316, 318, -1, 326, 517, 518, -1, 517, 519, 518, -1, 519, 515, 518, -1, 319, 326, 518, -1, 318, 319, 518, -1, 515, 318, 518, -1, 520, 303, 320, -1, 520, 320, 317, -1, 520, 317, 316, -1, 520, 521, 303, -1, 520, 522, 521, -1, 520, 523, 522, -1, 520, 516, 523, -1, 520, 316, 516, -1, 328, 333, 524, -1, 525, 328, 524, -1, 329, 328, 525, -1, 526, 323, 376, -1, 527, 329, 525, -1, 326, 527, 517, -1, 324, 526, 528, -1, 324, 323, 526, -1, 326, 329, 527, -1, 529, 324, 528, -1, 330, 529, 530, -1, 330, 324, 529, -1, 331, 530, 531, -1, 331, 330, 530, -1, 321, 531, 532, -1, 321, 331, 531, -1, 322, 321, 532, -1, 533, 322, 532, -1, 313, 322, 533, -1, 534, 313, 533, -1, 314, 313, 534, -1, 535, 314, 534, -1, 335, 314, 535, -1, 536, 335, 535, -1, 332, 335, 536, -1, 537, 332, 536, -1, 333, 332, 537, -1, 524, 333, 537, -1, 538, 539, 540, -1, 540, 539, 541, -1, 542, 543, 540, -1, 541, 542, 540, -1, 544, 375, 362, -1, 545, 352, 356, -1, 544, 546, 375, -1, 545, 353, 352, -1, 545, 369, 353, -1, 545, 547, 369, -1, 548, 354, 359, -1, 548, 359, 358, -1, 548, 358, 360, -1, 549, 361, 363, -1, 548, 356, 354, -1, 549, 362, 361, -1, 548, 545, 356, -1, 549, 544, 362, -1, 364, 379, 365, -1, 367, 379, 364, -1, 550, 382, 380, -1, 551, 370, 371, -1, 551, 368, 370, -1, 551, 367, 368, -1, 551, 380, 379, -1, 551, 550, 380, -1, 551, 379, 367, -1, 547, 366, 369, -1, 547, 363, 366, -1, 552, 371, 372, -1, 547, 549, 363, -1, 552, 551, 371, -1, 553, 372, 373, -1, 553, 552, 372, -1, 546, 374, 375, -1, 546, 373, 374, -1, 546, 553, 373, -1, 554, 387, 383, -1, 219, 555, 556, -1, 219, 554, 555, -1, 357, 556, 360, -1, 357, 219, 556, -1, 557, 219, 385, -1, 557, 385, 387, -1, 557, 387, 554, -1, 557, 554, 219, -1, 550, 558, 378, -1, 559, 536, 535, -1, 559, 560, 536, -1, 550, 378, 382, -1, 561, 534, 533, -1, 561, 559, 534, -1, 562, 533, 532, -1, 562, 561, 533, -1, 563, 515, 519, -1, 563, 516, 515, -1, 563, 521, 522, -1, 563, 522, 523, -1, 563, 523, 516, -1, 564, 527, 525, -1, 564, 517, 527, -1, 564, 519, 517, -1, 564, 563, 519, -1, 377, 526, 376, -1, 565, 524, 537, -1, 565, 525, 524, -1, 565, 564, 525, -1, 566, 531, 530, -1, 566, 532, 531, -1, 566, 562, 532, -1, 560, 537, 536, -1, 558, 377, 378, -1, 560, 565, 537, -1, 558, 528, 526, -1, 558, 529, 528, -1, 558, 530, 529, -1, 558, 526, 377, -1, 558, 566, 530, -1, 559, 535, 534, -1, 567, 384, 388, -1, 383, 567, 568, -1, 383, 384, 567, -1, 569, 383, 568, -1, 412, 570, 571, -1, 572, 412, 571, -1, 572, 571, 573, -1, 573, 571, 574, -1, 573, 574, 575, -1, 575, 574, 576, -1, 575, 576, 577, -1, 577, 576, 578, -1, 577, 578, 579, -1, 579, 578, 580, -1, 579, 580, 581, -1, 581, 580, 582, -1, 581, 582, 226, -1, 226, 582, 388, -1, 583, 412, 410, -1, 583, 410, 408, -1, 583, 408, 407, -1, 584, 579, 585, -1, 418, 572, 573, -1, 418, 573, 575, -1, 409, 412, 572, -1, 409, 572, 418, -1, 586, 579, 581, -1, 586, 585, 579, -1, 587, 581, 226, -1, 587, 586, 581, -1, 401, 587, 226, -1, 584, 420, 418, -1, 584, 588, 420, -1, 584, 585, 588, -1, 584, 575, 577, -1, 584, 577, 579, -1, 584, 418, 575, -1, 589, 590, 402, -1, 591, 403, 393, -1, 591, 592, 403, -1, 593, 402, 405, -1, 593, 589, 402, -1, 594, 393, 392, -1, 395, 593, 405, -1, 595, 396, 406, -1, 595, 596, 396, -1, 594, 591, 393, -1, 597, 593, 395, -1, 396, 597, 395, -1, 598, 406, 416, -1, 599, 392, 422, -1, 598, 595, 406, -1, 596, 597, 396, -1, 599, 594, 392, -1, 600, 599, 422, -1, 600, 422, 421, -1, 601, 416, 417, -1, 601, 598, 416, -1, 602, 600, 421, -1, 602, 421, 419, -1, 603, 417, 411, -1, 603, 601, 417, -1, 604, 602, 419, -1, 604, 419, 420, -1, 605, 411, 404, -1, 588, 604, 420, -1, 605, 603, 411, -1, 606, 604, 588, -1, 607, 404, 400, -1, 585, 606, 588, -1, 607, 605, 404, -1, 608, 606, 585, -1, 609, 400, 399, -1, 610, 585, 586, -1, 610, 608, 585, -1, 609, 607, 400, -1, 587, 610, 586, -1, 611, 399, 390, -1, 612, 610, 587, -1, 611, 609, 399, -1, 590, 587, 401, -1, 590, 612, 587, -1, 592, 390, 403, -1, 592, 611, 390, -1, 402, 590, 401, -1, 398, 233, 613, -1, 398, 613, 614, -1, 615, 391, 394, -1, 615, 394, 397, -1, 615, 397, 398, -1, 413, 391, 616, -1, 616, 391, 617, -1, 415, 414, 618, -1, 407, 415, 618, -1, 618, 414, 413, -1, 425, 619, 620, -1, 456, 425, 426, -1, 456, 621, 619, -1, 456, 619, 425, -1, 427, 425, 622, -1, 623, 620, 624, -1, 623, 624, 625, -1, 623, 625, 622, -1, 623, 425, 620, -1, 623, 622, 425, -1, 613, 233, 626, -1, 233, 427, 626, -1, 613, 626, 614, -1, 614, 626, 627, -1, 398, 614, 628, -1, 614, 627, 628, -1, 449, 446, 629, -1, 451, 449, 629, -1, 452, 451, 629, -1, 450, 452, 629, -1, 448, 450, 629, -1, 447, 448, 629, -1, 445, 447, 629, -1, 442, 445, 629, -1, 441, 442, 629, -1, 439, 441, 629, -1, 437, 439, 629, -1, 435, 437, 629, -1, 433, 435, 629, -1, 431, 433, 629, -1, 429, 431, 629, -1, 428, 429, 629, -1, 430, 428, 629, -1, 432, 430, 629, -1, 434, 432, 629, -1, 436, 434, 629, -1, 438, 436, 629, -1, 440, 438, 629, -1, 443, 440, 629, -1, 444, 443, 629, -1, 446, 444, 629, -1, 630, 383, 569, -1, 554, 383, 630, -1, 631, 554, 630, -1, 632, 554, 631, -1, 633, 554, 632, -1, 457, 621, 456, -1, 457, 634, 635, -1, 457, 635, 636, -1, 457, 636, 621, -1, 633, 634, 554, -1, 554, 634, 457, -1, 637, 554, 457, -1, 637, 638, 554, -1, 637, 457, 460, -1, 637, 639, 638, -1, 637, 460, 639, -1, 640, 460, 461, -1, 640, 639, 460, -1, 640, 550, 551, -1, 640, 551, 552, -1, 640, 552, 553, -1, 640, 553, 546, -1, 640, 546, 544, -1, 640, 544, 549, -1, 640, 549, 547, -1, 640, 547, 545, -1, 640, 563, 564, -1, 640, 564, 565, -1, 640, 565, 560, -1, 640, 560, 559, -1, 640, 559, 561, -1, 640, 561, 562, -1, 640, 562, 566, -1, 640, 566, 558, -1, 640, 558, 550, -1, 640, 461, 563, -1, 640, 545, 639, -1, 545, 548, 639, -1, 641, 508, 642, -1, 643, 642, 521, -1, 642, 508, 521, -1, 643, 521, 644, -1, 644, 521, 645, -1, 462, 646, 461, -1, 646, 647, 461, -1, 648, 645, 649, -1, 650, 648, 649, -1, 647, 650, 649, -1, 563, 461, 649, -1, 521, 563, 649, -1, 645, 521, 649, -1, 461, 647, 649, -1, 651, 652, 511, -1, 466, 511, 462, -1, 652, 646, 462, -1, 511, 652, 462, -1, 653, 511, 513, -1, 654, 651, 655, -1, 656, 654, 655, -1, 653, 656, 655, -1, 651, 511, 655, -1, 511, 653, 655, -1, 498, 499, 657, -1, 496, 498, 657, -1, 497, 496, 657, -1, 658, 659, 492, -1, 660, 658, 492, -1, 661, 492, 491, -1, 662, 663, 664, -1, 663, 665, 664, -1, 659, 499, 495, -1, 492, 659, 495, -1, 505, 662, 666, -1, 662, 664, 666, -1, 505, 666, 471, -1, 661, 665, 667, -1, 668, 660, 667, -1, 663, 668, 667, -1, 660, 492, 667, -1, 665, 663, 667, -1, 492, 661, 667, -1, 484, 669, 670, -1, 484, 469, 669, -1, 473, 671, 672, -1, 473, 478, 671, -1, 494, 670, 673, -1, 494, 484, 670, -1, 504, 672, 674, -1, 476, 675, 676, -1, 504, 473, 672, -1, 476, 475, 675, -1, 477, 673, 677, -1, 477, 494, 673, -1, 675, 477, 677, -1, 503, 674, 678, -1, 486, 676, 679, -1, 503, 504, 674, -1, 486, 476, 676, -1, 475, 477, 675, -1, 490, 678, 680, -1, 493, 679, 681, -1, 490, 503, 678, -1, 682, 490, 680, -1, 493, 486, 679, -1, 491, 490, 682, -1, 489, 681, 683, -1, 489, 493, 681, -1, 661, 682, 684, -1, 661, 491, 682, -1, 481, 683, 685, -1, 665, 684, 686, -1, 481, 489, 683, -1, 665, 661, 684, -1, 482, 685, 687, -1, 664, 686, 688, -1, 664, 665, 686, -1, 482, 481, 685, -1, 502, 687, 689, -1, 666, 688, 690, -1, 666, 664, 688, -1, 502, 482, 687, -1, 471, 690, 691, -1, 487, 689, 692, -1, 471, 666, 690, -1, 487, 502, 689, -1, 671, 487, 692, -1, 469, 691, 669, -1, 469, 471, 691, -1, 478, 487, 671, -1, 693, 304, 480, -1, 694, 693, 480, -1, 483, 472, 695, -1, 479, 483, 695, -1, 480, 479, 695, -1, 696, 472, 474, -1, 697, 696, 474, -1, 698, 501, 500, -1, 698, 500, 497, -1, 474, 501, 698, -1, 507, 506, 699, -1, 700, 699, 508, -1, 699, 506, 508, -1, 700, 508, 641, -1, 701, 499, 659, -1, 702, 701, 659, -1, 702, 659, 703, -1, 703, 659, 658, -1, 703, 658, 704, -1, 704, 658, 660, -1, 704, 660, 705, -1, 705, 660, 668, -1, 706, 705, 663, -1, 705, 668, 663, -1, 707, 706, 662, -1, 706, 663, 662, -1, 707, 662, 507, -1, 507, 662, 505, -1, 521, 510, 303, -1, 708, 508, 509, -1, 708, 509, 510, -1, 708, 521, 508, -1, 708, 510, 521, -1, 709, 513, 693, -1, 513, 304, 693, -1, 709, 693, 710, -1, 710, 693, 694, -1, 711, 710, 480, -1, 710, 694, 480, -1, 548, 360, 639, -1, 360, 638, 639, -1, 360, 556, 638, -1, 712, 554, 638, -1, 712, 556, 555, -1, 712, 555, 554, -1, 712, 638, 556, -1, 625, 611, 622, -1, 713, 594, 714, -1, 625, 609, 611, -1, 591, 594, 713, -1, 624, 609, 625, -1, 715, 591, 713, -1, 624, 607, 609, -1, 716, 591, 715, -1, 620, 605, 607, -1, 628, 591, 716, -1, 620, 607, 624, -1, 589, 569, 568, -1, 589, 568, 590, -1, 619, 603, 605, -1, 592, 591, 628, -1, 619, 605, 620, -1, 621, 595, 598, -1, 621, 598, 601, -1, 621, 601, 603, -1, 427, 592, 628, -1, 621, 603, 619, -1, 593, 569, 589, -1, 596, 595, 569, -1, 717, 718, 719, -1, 596, 569, 597, -1, 717, 720, 718, -1, 569, 595, 621, -1, 717, 570, 720, -1, 717, 721, 714, -1, 717, 722, 721, -1, 717, 719, 722, -1, 597, 569, 593, -1, 602, 723, 717, -1, 602, 604, 723, -1, 622, 592, 427, -1, 622, 611, 592, -1, 600, 602, 717, -1, 599, 600, 717, -1, 594, 717, 714, -1, 594, 599, 717, -1, 590, 567, 388, -1, 590, 568, 567, -1, 412, 724, 570, -1, 412, 583, 724, -1, 608, 580, 725, -1, 574, 571, 723, -1, 576, 574, 723, -1, 571, 570, 717, -1, 723, 571, 717, -1, 582, 580, 610, -1, 580, 608, 610, -1, 388, 582, 612, -1, 582, 610, 612, -1, 388, 612, 590, -1, 723, 604, 725, -1, 604, 606, 725, -1, 606, 608, 725, -1, 578, 576, 725, -1, 580, 578, 725, -1, 576, 723, 725, -1, 719, 583, 407, -1, 719, 724, 583, -1, 398, 628, 726, -1, 615, 398, 726, -1, 391, 615, 726, -1, 391, 726, 713, -1, 617, 391, 727, -1, 391, 713, 727, -1, 616, 617, 728, -1, 617, 727, 728, -1, 413, 616, 714, -1, 616, 728, 714, -1, 413, 714, 729, -1, 618, 413, 729, -1, 729, 719, 407, -1, 729, 407, 618, -1, 626, 427, 628, -1, 627, 626, 628, -1, 730, 630, 569, -1, 730, 631, 630, -1, 730, 632, 631, -1, 730, 633, 632, -1, 730, 634, 633, -1, 730, 635, 634, -1, 730, 636, 635, -1, 730, 621, 636, -1, 730, 569, 621, -1, 641, 642, 731, -1, 642, 643, 731, -1, 643, 644, 731, -1, 644, 645, 731, -1, 645, 648, 731, -1, 648, 650, 731, -1, 650, 647, 731, -1, 647, 646, 731, -1, 646, 641, 731, -1, 656, 689, 654, -1, 691, 700, 669, -1, 689, 687, 654, -1, 669, 700, 641, -1, 687, 685, 651, -1, 732, 672, 733, -1, 654, 687, 651, -1, 685, 683, 652, -1, 651, 685, 652, -1, 679, 676, 646, -1, 681, 679, 646, -1, 711, 734, 671, -1, 683, 681, 646, -1, 734, 735, 671, -1, 652, 683, 646, -1, 735, 733, 671, -1, 646, 676, 675, -1, 641, 646, 675, -1, 733, 672, 671, -1, 677, 641, 675, -1, 669, 641, 670, -1, 711, 671, 513, -1, 513, 671, 692, -1, 736, 682, 680, -1, 670, 641, 673, -1, 513, 692, 653, -1, 737, 738, 739, -1, 738, 740, 739, -1, 653, 692, 689, -1, 740, 701, 739, -1, 741, 742, 739, -1, 742, 737, 739, -1, 673, 641, 677, -1, 736, 680, 739, -1, 653, 689, 656, -1, 739, 680, 678, -1, 732, 741, 674, -1, 741, 739, 674, -1, 739, 678, 674, -1, 507, 699, 691, -1, 691, 699, 700, -1, 732, 674, 672, -1, 701, 657, 499, -1, 701, 743, 657, -1, 497, 743, 737, -1, 497, 657, 743, -1, 736, 702, 703, -1, 736, 703, 704, -1, 682, 736, 684, -1, 688, 706, 707, -1, 688, 686, 706, -1, 739, 701, 702, -1, 739, 702, 736, -1, 690, 707, 507, -1, 690, 688, 707, -1, 691, 690, 507, -1, 744, 686, 684, -1, 744, 704, 705, -1, 744, 705, 706, -1, 744, 736, 704, -1, 744, 706, 686, -1, 744, 684, 736, -1, 711, 480, 695, -1, 745, 711, 695, -1, 733, 745, 695, -1, 733, 695, 472, -1, 733, 472, 746, -1, 746, 472, 696, -1, 747, 746, 697, -1, 746, 696, 697, -1, 732, 747, 474, -1, 747, 697, 474, -1, 732, 474, 698, -1, 748, 732, 698, -1, 698, 497, 737, -1, 698, 737, 748, -1, 711, 513, 709, -1, 711, 709, 710, -1, 720, 570, 724, -1, 718, 720, 724, -1, 719, 718, 724, -1, 715, 713, 726, -1, 716, 715, 726, -1, 628, 716, 726, -1, 728, 713, 714, -1, 727, 713, 728, -1, 729, 721, 722, -1, 729, 722, 719, -1, 714, 721, 729, -1, 743, 701, 740, -1, 743, 740, 738, -1, 743, 738, 737, -1, 745, 733, 735, -1, 745, 735, 734, -1, 745, 734, 711, -1, 732, 733, 746, -1, 732, 746, 747, -1, 742, 741, 748, -1, 737, 742, 748, -1, 748, 741, 732, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position -0.500 -2.150 60.778 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position -0.500 -2.150 -106.278 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position -0.500 -85.678 -22.750 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position -0.500 81.377 -22.750 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -84.028 -2.150 -22.750 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 83.028 -2.150 -22.750 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/GR001/parts/WAIST.wrl000066400000000000000000003366661207742442300224760ustar00rootroot00000000000000#VRML V2.0 utf8 WorldInfo { title "Exported tringle mesh to VRML97" info ["Created by FreeCAD" ""] } Background { skyAngle 1.57 skyColor 0.1 0.2 0.2 } Transform { scale 0.001 0.001 0.001 rotation 0 0 1 0 scaleOrientation 0 0 1 0 bboxCenter 0 0 0 bboxSize 89.000 84.000 50.000 center 0.000 0.000 0.000 #translation -14.500 -0.000 -23.500 children [ Shape { appearance Appearance { material Material { ambientIntensity 0.2 diffuseColor 0.30 0.30 0.80 emissiveColor 0.00 0.00 0.00 # specularColor 0.98 0.98 0.98 shininess 0.06 transparency 0.00 } } geometry IndexedFaceSet { solid FALSE coord Coordinate { point [ 29.000 12.000 48.500, 29.000 -12.000 48.500, 57.500 -12.000 48.500, 57.500 12.000 48.500, 29.000 -12.000 9.500, 43.172 -12.000 18.172, 42.873 -12.000 18.506, 43.506 -12.000 17.873, 43.872 -12.000 17.613, 42.613 -12.000 18.872, 42.396 -12.000 19.264, 44.264 -12.000 17.396, 44.679 -12.000 17.224, 42.224 -12.000 19.679, 42.100 -12.000 20.110, 45.110 -12.000 17.100, 45.552 -12.000 17.025, 42.025 -12.000 20.552, 29.000 -12.000 32.500, 42.000 -12.000 21.000, 42.025 -12.000 21.448, 42.100 -12.000 21.890, 42.224 -12.000 22.321, 42.396 -12.000 22.736, 46.000 -12.000 17.000, 57.500 -12.000 9.500, 47.321 -12.000 17.224, 46.890 -12.000 17.100, 46.448 -12.000 17.025, 47.736 -12.000 17.396, 57.500 -12.000 10.066, 57.500 -12.000 12.917, 49.387 -12.000 18.872, 49.127 -12.000 18.506, 48.828 -12.000 18.172, 48.494 -12.000 17.873, 48.128 -12.000 17.613, 57.500 -12.000 19.500, 48.828 -12.000 23.828, 49.127 -12.000 23.494, 49.387 -12.000 23.128, 49.604 -12.000 22.736, 49.776 -12.000 22.321, 49.900 -12.000 21.890, 49.975 -12.000 21.448, 50.000 -12.000 21.000, 49.975 -12.000 20.552, 49.900 -12.000 20.110, 49.776 -12.000 19.679, 49.604 -12.000 19.264, 48.128 -12.000 24.387, 48.494 -12.000 24.127, 29.000 -12.000 39.500, 43.250 -12.000 29.000, 42.613 -12.000 23.128, 42.873 -12.000 23.494, 43.172 -12.000 23.828, 43.506 -12.000 24.127, 43.872 -12.000 24.387, 44.264 -12.000 24.604, 44.679 -12.000 24.776, 45.110 -12.000 24.900, 45.552 -12.000 24.975, 46.000 -12.000 25.000, 46.448 -12.000 24.975, 46.890 -12.000 24.900, 47.321 -12.000 24.776, 47.736 -12.000 24.604, 29.000 12.000 39.500, 39.825 12.000 15.914, 40.422 12.000 15.266, 29.000 12.000 9.500, 41.086 12.000 14.687, 39.303 12.000 16.624, 41.811 12.000 14.185, 38.862 12.000 17.388, 42.586 12.000 13.765, 38.508 12.000 18.195, 43.402 12.000 13.433, 38.245 12.000 19.036, 29.000 12.000 25.500, 44.251 12.000 13.194, 38.076 12.000 19.901, 45.120 12.000 13.049, 38.003 12.000 20.780, 38.027 12.000 21.661, 38.148 12.000 22.534, 38.365 12.000 23.388, 38.674 12.000 24.214, 39.072 12.000 25.000, 39.554 12.000 25.738, 40.114 12.000 26.418, 57.500 12.000 9.500, 46.000 12.000 13.000, 49.414 12.000 13.765, 50.189 12.000 14.185, 48.598 12.000 13.433, 47.749 12.000 13.194, 46.880 12.000 13.049, 57.500 12.000 9.644, 50.914 12.000 14.687, 40.746 12.000 27.033, 41.442 12.000 27.574, 57.500 12.000 13.900, 52.697 12.000 16.624, 53.138 12.000 17.388, 52.175 12.000 15.914, 51.578 12.000 15.266, 53.492 12.000 18.195, 57.500 12.000 19.500, 53.973 12.000 21.661, 53.852 12.000 22.534, 53.997 12.000 20.780, 53.924 12.000 19.901, 53.755 12.000 19.036, 53.635 12.000 23.388, 53.326 12.000 24.214, 52.928 12.000 25.000, 46.441 12.000 28.988, 45.559 12.000 28.988, 47.317 12.000 28.891, 48.177 12.000 28.698, 49.010 12.000 28.412, 49.808 12.000 28.036, 50.558 12.000 27.574, 51.254 12.000 27.033, 51.886 12.000 26.418, 52.446 12.000 25.738, 43.250 12.000 29.000, 42.192 12.000 28.036, 42.990 12.000 28.412, 43.823 12.000 28.698, 44.683 12.000 28.891, 45.552 -13.000 17.025, 46.000 -13.000 17.000, 45.110 -13.000 17.100, 44.679 -13.000 17.224, 44.264 -13.000 17.396, 43.872 -13.000 17.613, 43.506 -13.000 17.873, 43.172 -13.000 18.172, 42.873 -13.000 18.506, 42.613 -13.000 18.872, 42.396 -13.000 19.264, 42.224 -13.000 19.679, 42.100 -13.000 20.110, 42.025 -13.000 20.552, 42.000 -13.000 21.000, 42.025 -13.000 21.448, 42.100 -13.000 21.890, 42.224 -13.000 22.321, 42.396 -13.000 22.736, 42.613 -13.000 23.128, 42.873 -13.000 23.494, 43.172 -13.000 23.828, 43.506 -13.000 24.127, 43.872 -13.000 24.387, 44.264 -13.000 24.604, 44.679 -13.000 24.776, 45.110 -13.000 24.900, 45.552 -13.000 24.975, 46.000 -13.000 25.000, 46.448 -13.000 24.975, 46.890 -13.000 24.900, 47.321 -13.000 24.776, 47.736 -13.000 24.604, 48.128 -13.000 24.387, 48.494 -13.000 24.127, 48.828 -13.000 23.828, 49.127 -13.000 23.494, 49.387 -13.000 23.128, 49.604 -13.000 22.736, 49.776 -13.000 22.321, 49.900 -13.000 21.890, 49.975 -13.000 21.448, 50.000 -13.000 21.000, 49.975 -13.000 20.552, 49.900 -13.000 20.110, 49.776 -13.000 19.679, 49.604 -13.000 19.264, 49.387 -13.000 18.872, 49.127 -13.000 18.506, 48.828 -13.000 18.172, 48.494 -13.000 17.873, 48.128 -13.000 17.613, 47.736 -13.000 17.396, 47.321 -13.000 17.224, 46.890 -13.000 17.100, 46.448 -13.000 17.025, 59.000 -12.000 10.066, 59.000 -12.000 9.500, 57.500 -12.396 9.500, 29.000 -12.396 9.500, 59.000 -12.000 12.917, 29.000 -33.000 -0.500, 29.000 -19.000 -0.500, 29.000 -19.000 0.069, 29.000 -33.000 1.000, 29.000 -33.000 2.500, 29.000 -12.396 9.500, 29.000 -22.500 16.000, 29.000 -12.000 9.500, 29.000 -12.000 32.500, 29.000 -33.000 32.500, 59.000 -12.000 19.500, -18.000 -12.000 32.500, -18.000 -12.000 39.500, -18.000 12.000 39.500, 45.120 13.000 13.049, 46.000 13.000 13.000, 44.251 13.000 13.194, 43.402 13.000 13.433, 42.586 13.000 13.765, 41.811 13.000 14.185, 41.086 13.000 14.687, 40.422 13.000 15.266, 39.825 13.000 15.914, 39.303 13.000 16.624, 38.862 13.000 17.388, 38.508 13.000 18.195, 38.245 13.000 19.036, 38.076 13.000 19.901, 38.003 13.000 20.780, 38.027 13.000 21.661, 38.148 13.000 22.534, 38.365 13.000 23.388, 38.674 13.000 24.214, 39.072 13.000 25.000, 39.554 13.000 25.738, 40.114 13.000 26.418, 40.746 13.000 27.033, 41.442 13.000 27.574, 42.192 13.000 28.036, 42.990 13.000 28.412, 43.823 13.000 28.698, 44.683 13.000 28.891, 45.559 13.000 28.988, 46.441 13.000 28.988, 47.317 13.000 28.891, 48.177 13.000 28.698, 49.010 13.000 28.412, 49.808 13.000 28.036, 50.558 13.000 27.574, 51.254 13.000 27.033, 51.886 13.000 26.418, 52.446 13.000 25.738, 52.928 13.000 25.000, 53.326 13.000 24.214, 53.635 13.000 23.388, 53.852 13.000 22.534, 53.973 13.000 21.661, 53.997 13.000 20.780, 53.924 13.000 19.901, 53.755 13.000 19.036, 53.492 13.000 18.195, 53.138 13.000 17.388, 52.697 13.000 16.624, 52.175 13.000 15.914, 51.578 13.000 15.266, 50.914 13.000 14.687, 50.189 13.000 14.185, 49.414 13.000 13.765, 48.598 13.000 13.433, 47.749 13.000 13.194, 46.880 13.000 13.049, 59.000 12.000 9.644, 59.000 12.000 9.500, 29.000 12.101 9.500, 57.500 12.101 9.500, 59.000 12.000 13.900, 29.000 19.000 -0.353, 29.000 19.000 -0.500, 29.000 33.000 -0.500, 29.000 33.000 25.500, 29.000 22.500 12.500, 59.000 12.000 19.500, -18.000 12.000 25.500, 45.552 -13.000 17.025, 46.000 -13.000 21.000, 45.110 -13.000 17.100, 44.679 -13.000 17.224, 44.264 -13.000 17.396, 43.872 -13.000 17.613, 43.506 -13.000 17.873, 43.172 -13.000 18.172, 42.873 -13.000 18.506, 42.613 -13.000 18.872, 42.396 -13.000 19.264, 42.224 -13.000 19.679, 42.100 -13.000 20.110, 42.025 -13.000 20.552, 42.025 -13.000 21.448, 42.100 -13.000 21.890, 42.224 -13.000 22.321, 42.396 -13.000 22.736, 42.613 -13.000 23.128, 42.873 -13.000 23.494, 43.172 -13.000 23.828, 43.506 -13.000 24.127, 43.872 -13.000 24.387, 44.264 -13.000 24.604, 44.679 -13.000 24.776, 45.110 -13.000 24.900, 45.552 -13.000 24.975, 46.448 -13.000 24.975, 46.890 -13.000 24.900, 47.321 -13.000 24.776, 47.736 -13.000 24.604, 48.128 -13.000 24.387, 48.494 -13.000 24.127, 48.828 -13.000 23.828, 49.127 -13.000 23.494, 49.387 -13.000 23.128, 49.604 -13.000 22.736, 49.776 -13.000 22.321, 49.900 -13.000 21.890, 49.975 -13.000 21.448, 50.000 -13.000 21.000, 49.975 -13.000 20.552, 49.900 -13.000 20.110, 49.776 -13.000 19.679, 49.604 -13.000 19.264, 48.128 -13.000 17.613, 47.321 -13.000 17.224, 46.890 -13.000 17.100, 46.448 -13.000 17.025, 59.000 -12.396 9.500, 59.000 -10.661 11.979, 59.000 10.000 12.500, 59.000 -0.000 14.500, 59.000 -12.874 8.818, 51.395 -19.000 0.069, 44.000 -15.698 4.784, 29.000 -19.000 0.069, 29.000 -19.000 -0.500, 40.197 -19.000 -0.216, 51.023 -19.000 -0.500, 50.953 -19.000 -0.439, 4.000 -14.072 -0.500, 3.214 -13.674 -0.500, 5.500 0.000 -0.500, 2.388 -13.365 -0.500, 1.534 -13.148 -0.500, 0.661 -13.027 -0.500, -0.220 -13.003 -0.500, -1.099 -13.076 -0.500, -1.964 -13.245 -0.500, -2.805 -13.508 -0.500, 28.000 -19.000 -0.500, 6.574 -16.442 -0.500, -3.612 -13.862 -0.500, 6.574 16.442 -0.500, 28.000 19.000 -0.500, -3.612 13.862 -0.500, -2.805 13.508 -0.500, -1.964 13.245 -0.500, -1.099 13.076 -0.500, 29.000 -33.000 -0.500, 16.500 -33.000 -0.500, -0.220 13.003 -0.500, 0.661 13.027 -0.500, 1.534 13.148 -0.500, 2.388 13.365 -0.500, 3.214 13.674 -0.500, 4.000 14.072 -0.500, 4.738 14.554 -0.500, 5.418 15.114 -0.500, 6.033 15.746 -0.500, 5.418 -26.886 -0.500, 6.033 -26.254 -0.500, 4.738 -27.446 -0.500, 6.574 -25.558 -0.500, 4.000 -27.928 -0.500, 7.036 -24.808 -0.500, 3.214 -28.326 -0.500, 7.412 -24.010 -0.500, 2.388 -28.635 -0.500, 7.698 -23.177 -0.500, 1.534 -28.852 -0.500, 7.891 -22.317 -0.500, 0.661 -28.973 -0.500, 7.988 -21.441 -0.500, 7.988 -20.559 -0.500, 7.891 -19.683 -0.500, 7.698 -18.823 -0.500, 7.412 -17.990 -0.500, 7.036 -17.192 -0.500, -3.612 -28.138 -0.500, -2.805 -28.492 -0.500, -9.500 -33.000 -0.500, -1.964 -28.755 -0.500, -1.099 -28.924 -0.500, -0.220 -28.997 -0.500, -4.376 -27.697 -0.500, -5.086 -27.175 -0.500, -6.815 -25.189 -0.500, -6.313 -25.914 -0.500, -9.500 -27.000 -0.500, -5.734 -26.578 -0.500, -7.235 -24.414 -0.500, -7.567 -23.598 -0.500, -7.806 -22.749 -0.500, -7.951 -21.880 -0.500, -8.000 -21.000 -0.500, -18.000 -27.000 -0.500, -6.815 -16.811 -0.500, -7.235 -17.586 -0.500, -7.567 -18.402 -0.500, -7.806 -19.251 -0.500, -7.951 -20.120 -0.500, 7.036 17.192 -0.500, 7.412 17.990 -0.500, 7.698 18.823 -0.500, 7.891 19.683 -0.500, 7.988 20.559 -0.500, 7.988 21.441 -0.500, 16.500 33.000 -0.500, 7.891 22.317 -0.500, 7.698 23.177 -0.500, 7.412 24.010 -0.500, -4.376 -14.303 -0.500, 7.036 24.808 -0.500, -5.086 -14.825 -0.500, -4.376 14.303 -0.500, 6.574 25.558 -0.500, -5.086 14.825 -0.500, -5.734 -15.422 -0.500, 6.033 26.254 -0.500, -5.734 15.422 -0.500, -6.313 -16.086 -0.500, 5.418 26.886 -0.500, -6.313 16.086 -0.500, 4.738 27.446 -0.500, 4.000 27.928 -0.500, 3.214 28.326 -0.500, 2.388 28.635 -0.500, 1.534 28.852 -0.500, 0.661 28.973 -0.500, -5.734 26.578 -0.500, -6.313 25.914 -0.500, -9.500 26.000 -0.500, -6.815 25.189 -0.500, -7.235 24.414 -0.500, -7.567 23.598 -0.500, -7.806 22.749 -0.500, -7.951 21.880 -0.500, -0.220 28.997 -0.500, -1.099 28.924 -0.500, -9.500 33.000 -0.500, -1.964 28.755 -0.500, -2.805 28.492 -0.500, -3.612 28.138 -0.500, -4.376 27.697 -0.500, -5.086 27.175 -0.500, -8.000 21.000 -0.500, -18.000 26.000 -0.500, -7.951 20.120 -0.500, -7.806 19.251 -0.500, -7.567 18.402 -0.500, -7.235 17.586 -0.500, -6.815 16.811 -0.500, 6.033 -15.746 -0.500, 5.418 -15.114 -0.500, 4.738 -14.554 -0.500, 16.500 -33.000 1.000, 16.500 -33.000 -0.500, -18.000 -33.000 32.500, 29.000 -33.000 32.500, 33.779 -33.000 2.500, 33.779 -33.000 1.000, -18.000 -33.000 22.500, -9.500 -33.000 22.500, -18.000 -33.000 32.500, -9.500 -33.000 2.500, 5.500 -33.000 17.500, -18.000 -27.000 22.500, -18.000 12.000 39.500, -18.000 12.000 25.500, -18.000 26.000 25.500, -18.000 26.000 -0.500, -18.000 -3.500 19.500, 46.749 13.000 21.663, 51.886 13.000 26.418, 52.446 13.000 25.738, 46.663 13.000 21.749, 51.254 13.000 27.033, 46.823 13.000 21.568, 52.928 13.000 25.000, 46.568 13.000 21.823, 49.808 13.000 28.036, 50.558 13.000 27.574, 46.465 13.000 21.885, 49.010 13.000 28.412, 46.885 13.000 21.465, 53.326 13.000 24.214, 46.935 13.000 21.355, 53.635 13.000 23.388, 46.355 13.000 21.935, 48.177 13.000 28.698, 46.971 13.000 21.239, 53.852 13.000 22.534, 46.239 13.000 21.971, 47.317 13.000 28.891, 46.993 13.000 21.121, 53.973 13.000 21.661, 46.121 13.000 21.993, 46.441 13.000 28.988, 46.000 13.000 22.000, 45.559 13.000 28.988, 47.000 13.000 21.000, 53.997 13.000 20.780, 45.879 13.000 21.993, 44.683 13.000 28.891, 46.993 13.000 20.879, 53.924 13.000 19.901, 46.971 13.000 20.761, 53.755 13.000 19.036, 45.761 13.000 21.971, 43.823 13.000 28.698, 46.935 13.000 20.645, 53.492 13.000 18.195, 53.138 13.000 17.388, 45.645 13.000 21.935, 42.990 13.000 28.412, 46.885 13.000 20.535, 52.697 13.000 16.624, 45.535 13.000 21.885, 42.192 13.000 28.036, 46.823 13.000 20.432, 52.175 13.000 15.914, 45.432 13.000 21.823, 40.746 13.000 27.033, 41.442 13.000 27.574, 46.749 13.000 20.337, 45.337 13.000 21.749, 51.578 13.000 15.266, 46.663 13.000 20.251, 40.114 13.000 26.418, 45.251 13.000 21.663, 50.914 13.000 14.687, 46.568 13.000 20.177, 39.554 13.000 25.738, 45.177 13.000 21.568, 50.189 13.000 14.185, 46.465 13.000 20.115, 39.072 13.000 25.000, 45.115 13.000 21.465, 46.355 13.000 20.065, 38.674 13.000 24.214, 45.065 13.000 21.355, 46.239 13.000 20.029, 38.365 13.000 23.388, 45.029 13.000 21.239, 47.749 13.000 13.194, 46.121 13.000 20.007, 38.148 13.000 22.534, 45.007 13.000 21.121, 46.880 13.000 13.049, 46.000 13.000 20.000, 38.027 13.000 21.661, 45.000 13.000 21.000, 46.000 13.000 13.000, 38.003 13.000 20.780, 45.007 13.000 20.879, 45.120 13.000 13.049, 45.879 13.000 20.007, 38.076 13.000 19.901, 45.029 13.000 20.761, 44.251 13.000 13.194, 45.761 13.000 20.029, 38.245 13.000 19.036, 45.065 13.000 20.645, 43.402 13.000 13.433, 45.645 13.000 20.065, 38.508 13.000 18.195, 42.586 13.000 13.765, 45.535 13.000 20.115, 38.862 13.000 17.388, 45.115 13.000 20.535, 41.811 13.000 14.185, 45.432 13.000 20.177, 39.303 13.000 16.624, 45.177 13.000 20.432, 41.086 13.000 14.687, 45.337 13.000 20.251, 39.825 13.000 15.914, 45.251 13.000 20.337, 40.422 13.000 15.266, 59.000 12.101 9.500, 59.000 12.578 8.818, 51.028 19.000 -0.353, 44.000 15.550 4.573, 16.500 33.000 1.000, 9.750 33.000 12.500, -9.500 33.000 25.500, -9.500 33.000 1.000, -18.000 26.000 25.500, -9.500 26.000 25.500, 5.500 22.500 25.500, 40.014 19.000 -0.427, 51.023 19.000 -0.500, 50.953 19.000 -0.439, 59.000 -0.148 9.159, 54.976 -0.000 4.189, -8.000 -21.000 -1.500, -7.951 -21.880 -1.500, -7.806 -22.749 -1.500, -7.567 -23.598 -1.500, -7.235 -24.414 -1.500, -6.815 -25.189 -1.500, -6.313 -25.914 -1.500, -5.734 -26.578 -1.500, -5.086 -27.175 -1.500, -4.376 -27.697 -1.500, -3.612 -28.138 -1.500, -2.805 -28.492 -1.500, -1.964 -28.755 -1.500, -1.099 -28.924 -1.500, -0.220 -28.997 -1.500, 0.661 -28.973 -1.500, 1.534 -28.852 -1.500, 2.388 -28.635 -1.500, 3.214 -28.326 -1.500, 4.000 -27.928 -1.500, 4.738 -27.446 -1.500, 5.418 -26.886 -1.500, 6.033 -26.254 -1.500, 6.574 -25.558 -1.500, 7.036 -24.808 -1.500, 7.412 -24.010 -1.500, 7.698 -23.177 -1.500, 7.891 -22.317 -1.500, 7.988 -21.441 -1.500, 7.988 -20.559 -1.500, 7.891 -19.683 -1.500, 7.698 -18.823 -1.500, 7.412 -17.990 -1.500, 7.036 -17.192 -1.500, 6.574 -16.442 -1.500, 6.033 -15.746 -1.500, 5.418 -15.114 -1.500, 4.738 -14.554 -1.500, 4.000 -14.072 -1.500, 3.214 -13.674 -1.500, 2.388 -13.365 -1.500, 1.534 -13.148 -1.500, 0.661 -13.027 -1.500, -0.220 -13.003 -1.500, -1.099 -13.076 -1.500, -1.964 -13.245 -1.500, -2.805 -13.508 -1.500, -3.612 -13.862 -1.500, -4.376 -14.303 -1.500, -5.086 -14.825 -1.500, -5.734 -15.422 -1.500, -6.313 -16.086 -1.500, -6.815 -16.811 -1.500, -7.235 -17.586 -1.500, -7.567 -18.402 -1.500, -7.806 -19.251 -1.500, -7.951 -20.120 -1.500, -8.000 21.000 -1.500, -7.951 20.120 -1.500, -7.806 19.251 -1.500, -7.567 18.402 -1.500, -7.235 17.586 -1.500, -6.815 16.811 -1.500, -6.313 16.086 -1.500, -5.734 15.422 -1.500, -5.086 14.825 -1.500, -4.376 14.303 -1.500, -3.612 13.862 -1.500, -2.805 13.508 -1.500, -1.964 13.245 -1.500, -1.099 13.076 -1.500, -0.220 13.003 -1.500, 0.661 13.027 -1.500, 1.534 13.148 -1.500, 2.388 13.365 -1.500, 3.214 13.674 -1.500, 4.000 14.072 -1.500, 4.738 14.554 -1.500, 5.418 15.114 -1.500, 6.033 15.746 -1.500, 6.574 16.442 -1.500, 7.036 17.192 -1.500, 7.412 17.990 -1.500, 7.698 18.823 -1.500, 7.891 19.683 -1.500, 7.988 20.559 -1.500, 7.988 21.441 -1.500, 7.891 22.317 -1.500, 7.698 23.177 -1.500, 7.412 24.010 -1.500, 7.036 24.808 -1.500, 6.574 25.558 -1.500, 6.033 26.254 -1.500, 5.418 26.886 -1.500, 4.738 27.446 -1.500, 4.000 27.928 -1.500, 3.214 28.326 -1.500, 2.388 28.635 -1.500, 1.534 28.852 -1.500, 0.661 28.973 -1.500, -0.220 28.997 -1.500, -1.099 28.924 -1.500, -1.964 28.755 -1.500, -2.805 28.492 -1.500, -3.612 28.138 -1.500, -4.376 27.697 -1.500, -5.086 27.175 -1.500, -5.734 26.578 -1.500, -6.313 25.914 -1.500, -6.815 25.189 -1.500, -7.235 24.414 -1.500, -7.567 23.598 -1.500, -7.806 22.749 -1.500, -7.951 21.880 -1.500, 16.500 -37.208 -0.500, 4.016 -42.000 -0.500, 3.500 -37.500 -0.500, -9.500 -42.000 -0.500, -9.500 -27.000 -0.500, -9.500 -33.000 -0.500, -9.500 -33.000 1.000, -9.500 -30.000 11.000, -9.500 -27.000 22.500, -9.500 -27.000 22.500, -9.500 29.500 12.500, 3.500 37.500 -0.500, 4.016 42.000 -0.500, 16.500 37.208 -0.500, -9.500 42.000 -0.500, 28.279 -38.500 1.000, 13.134 -38.500 1.000, 16.500 -37.208 1.000, 23.456 -35.750 1.000, 16.500 -37.208 -0.500, 28.279 -38.500 2.500, -15.500 -38.500 2.500, -15.000 -35.000 2.500, -17.000 -37.000 2.500, -17.000 -33.000 2.500, 8.389 -35.750 2.500, 33.779 -33.000 1.000, 33.779 -33.000 2.500, 33.900 -33.000 2.500, 33.900 -33.000 1.000, -9.500 -33.000 22.500, -18.000 -33.000 22.500, 46.000 13.000 21.000, 16.500 37.208 1.000, 4.016 42.000 1.000, 3.500 37.500 1.000, -9.500 42.000 1.000, 4.738 -14.554 -1.500, 5.418 -15.114 -1.500, 0.663 -20.251 -1.500, 0.749 -20.337 -1.500, 6.033 -15.746 -1.500, 0.568 -20.177 -1.500, 4.000 -14.072 -1.500, 0.823 -20.432 -1.500, 6.574 -16.442 -1.500, 7.036 -17.192 -1.500, 0.885 -20.535 -1.500, 7.412 -17.990 -1.500, 0.465 -20.115 -1.500, 3.214 -13.674 -1.500, 0.355 -20.065 -1.500, 2.388 -13.365 -1.500, 0.935 -20.645 -1.500, 7.698 -18.823 -1.500, 0.239 -20.029 -1.500, 1.534 -13.148 -1.500, 0.971 -20.761 -1.500, 7.891 -19.683 -1.500, 0.121 -20.007 -1.500, 0.661 -13.027 -1.500, 7.988 -20.559 -1.500, 0.993 -20.879 -1.500, 7.988 -21.441 -1.500, 1.000 -21.000 -1.500, 0.000 -20.000 -1.500, -0.220 -13.003 -1.500, 7.891 -22.317 -1.500, 0.993 -21.121 -1.500, -0.121 -20.007 -1.500, -1.099 -13.076 -1.500, -0.239 -20.029 -1.500, -1.964 -13.245 -1.500, 0.971 -21.239 -1.500, 7.698 -23.177 -1.500, -0.355 -20.065 -1.500, -2.805 -13.508 -1.500, -3.612 -13.862 -1.500, 0.935 -21.355 -1.500, 7.412 -24.010 -1.500, -0.465 -20.115 -1.500, -4.376 -14.303 -1.500, 0.885 -21.465 -1.500, 7.036 -24.808 -1.500, -5.086 -14.825 -1.500, -0.568 -20.177 -1.500, 0.823 -21.568 -1.500, 6.574 -25.558 -1.500, 6.033 -26.254 -1.500, -0.663 -20.251 -1.500, 0.749 -21.663 -1.500, -5.734 -15.422 -1.500, -0.749 -20.337 -1.500, 5.418 -26.886 -1.500, 0.663 -21.749 -1.500, -6.313 -16.086 -1.500, -0.823 -20.432 -1.500, 0.568 -21.823 -1.500, 4.738 -27.446 -1.500, -6.815 -16.811 -1.500, -0.885 -20.535 -1.500, 0.465 -21.885 -1.500, 4.000 -27.928 -1.500, -7.235 -17.586 -1.500, -0.935 -20.645 -1.500, 0.355 -21.935 -1.500, 3.214 -28.326 -1.500, -7.567 -18.402 -1.500, -0.971 -20.761 -1.500, 0.239 -21.971 -1.500, 2.388 -28.635 -1.500, -0.993 -20.879 -1.500, -7.806 -19.251 -1.500, 0.121 -21.993 -1.500, 1.534 -28.852 -1.500, -1.000 -21.000 -1.500, -7.951 -20.120 -1.500, -0.000 -22.000 -1.500, 0.661 -28.973 -1.500, -8.000 -21.000 -1.500, -0.121 -21.993 -1.500, -0.220 -28.997 -1.500, -0.993 -21.121 -1.500, -7.951 -21.880 -1.500, -0.239 -21.971 -1.500, -1.099 -28.924 -1.500, -0.971 -21.239 -1.500, -7.806 -22.749 -1.500, -1.964 -28.755 -1.500, -0.355 -21.935 -1.500, -0.935 -21.355 -1.500, -7.567 -23.598 -1.500, -2.805 -28.492 -1.500, -0.885 -21.465 -1.500, -7.235 -24.414 -1.500, -3.612 -28.138 -1.500, -0.465 -21.885 -1.500, -6.815 -25.189 -1.500, -0.823 -21.568 -1.500, -0.568 -21.823 -1.500, -4.376 -27.697 -1.500, -0.749 -21.663 -1.500, -6.313 -25.914 -1.500, -0.663 -21.749 -1.500, -5.086 -27.175 -1.500, -5.734 -26.578 -1.500, 4.738 27.446 -1.500, 5.418 26.886 -1.500, 0.663 21.749 -1.500, 0.749 21.663 -1.500, 6.033 26.254 -1.500, 0.568 21.823 -1.500, 4.000 27.928 -1.500, 0.823 21.568 -1.500, 6.574 25.558 -1.500, 7.036 24.808 -1.500, 0.885 21.465 -1.500, 7.412 24.010 -1.500, 0.465 21.885 -1.500, 3.214 28.326 -1.500, 0.355 21.935 -1.500, 2.388 28.635 -1.500, 0.935 21.355 -1.500, 7.698 23.177 -1.500, 0.239 21.971 -1.500, 1.534 28.852 -1.500, 0.971 21.239 -1.500, 7.891 22.317 -1.500, 0.121 21.993 -1.500, 0.661 28.973 -1.500, 7.988 21.441 -1.500, 0.993 21.121 -1.500, 7.988 20.559 -1.500, 1.000 21.000 -1.500, 0.000 22.000 -1.500, -0.220 28.997 -1.500, 7.891 19.683 -1.500, 0.993 20.879 -1.500, -0.121 21.993 -1.500, -1.099 28.924 -1.500, -0.239 21.971 -1.500, -1.964 28.755 -1.500, 0.971 20.761 -1.500, 7.698 18.823 -1.500, -0.355 21.935 -1.500, -2.805 28.492 -1.500, -3.612 28.138 -1.500, 0.935 20.645 -1.500, 7.412 17.990 -1.500, -0.465 21.885 -1.500, -4.376 27.697 -1.500, 0.885 20.535 -1.500, 7.036 17.192 -1.500, -5.086 27.175 -1.500, -0.568 21.823 -1.500, 0.823 20.432 -1.500, 6.574 16.442 -1.500, 6.033 15.746 -1.500, -0.663 21.749 -1.500, 0.749 20.337 -1.500, -5.734 26.578 -1.500, -0.749 21.663 -1.500, 5.418 15.114 -1.500, 0.663 20.251 -1.500, -6.313 25.914 -1.500, -0.823 21.568 -1.500, 0.568 20.177 -1.500, 4.738 14.554 -1.500, -6.815 25.189 -1.500, -0.885 21.465 -1.500, 0.465 20.115 -1.500, 4.000 14.072 -1.500, -7.235 24.414 -1.500, -0.935 21.355 -1.500, 0.355 20.065 -1.500, 3.214 13.674 -1.500, -7.567 23.598 -1.500, -0.971 21.239 -1.500, 0.239 20.029 -1.500, 2.388 13.365 -1.500, -0.993 21.121 -1.500, -7.806 22.749 -1.500, 0.121 20.007 -1.500, 1.534 13.148 -1.500, -1.000 21.000 -1.500, -7.951 21.880 -1.500, 0.000 20.000 -1.500, 0.661 13.027 -1.500, -8.000 21.000 -1.500, -0.121 20.007 -1.500, -0.220 13.003 -1.500, -0.993 20.879 -1.500, -7.951 20.120 -1.500, -0.239 20.029 -1.500, -1.099 13.076 -1.500, -0.971 20.761 -1.500, -7.806 19.251 -1.500, -1.964 13.245 -1.500, -0.355 20.065 -1.500, -0.935 20.645 -1.500, -7.567 18.402 -1.500, -2.805 13.508 -1.500, -0.885 20.535 -1.500, -7.235 17.586 -1.500, -3.612 13.862 -1.500, -0.465 20.115 -1.500, -6.815 16.811 -1.500, -0.823 20.432 -1.500, -0.568 20.177 -1.500, -4.376 14.303 -1.500, -0.749 20.337 -1.500, -6.313 16.086 -1.500, -0.663 20.251 -1.500, -5.086 14.825 -1.500, -5.734 15.422 -1.500, 4.016 -42.000 1.000, 4.016 -42.000 -0.500, 9.226 -40.000 1.000, 10.258 -39.604 0.250, 4.016 -42.000 1.000, -9.500 -42.000 1.000, -9.500 -40.000 1.000, -9.500 -42.000 -0.500, -9.500 -42.000 1.000, -9.500 -37.500 0.250, -9.500 -38.500 1.000, -17.000 -33.000 1.000, 16.500 37.208 -0.500, 4.016 42.000 -0.500, 4.016 42.000 1.000, 16.500 37.208 1.000, -9.500 42.000 -0.500, -9.500 42.000 1.000, 34.000 -33.100 1.000, 34.000 -34.900 1.000, 33.900 -33.000 1.000, 31.139 -36.450 1.000, 29.000 -39.900 1.000, 29.000 -38.500 1.000, 19.053 -39.250 1.000, 27.829 -38.950 1.000, 28.879 -40.000 1.000, 6.389 -38.500 14.094, -15.500 -38.500 25.687, 28.279 -38.500 25.687, 28.279 -38.500 6.000, -15.500 -38.500 2.500, 28.279 -38.500 2.500, -26.000 -28.000 25.640, -23.812 -30.188 25.605, -26.000 -28.000 15.000, -15.725 -38.275 25.660, -15.725 -38.275 25.955, -26.500 -27.500 2.500, -26.500 -27.500 14.500, -24.071 -29.929 2.500, -21.000 -33.000 14.228, -17.000 -37.000 2.500, -21.000 -33.000 2.500, -21.000 -33.000 2.500, 33.779 -33.000 6.000, -21.000 -33.000 2.500, 33.900 -33.000 6.000, 34.000 -33.000 2.500, 34.000 -33.000 1.000, 0.000 -21.000 -1.500, 0.000 21.000 -1.500, -0.137 -41.000 1.000, -17.000 -37.000 1.000, -15.000 -35.000 1.000, -15.500 -38.500 1.000, -13.250 -35.750 1.000, -16.000 -40.000 1.000, -15.000 -39.000 1.000, -16.000 -40.000 1.000, -12.750 -39.250 1.000, -21.000 -29.000 1.000, -21.000 -29.000 2.500, 34.000 -33.100 1.000, 34.900 -34.000 1.000, 34.000 -34.900 1.000, 29.000 -39.900 1.000, 29.000 -40.000 1.000, 29.000 -40.000 1.000, 31.500 -37.450 1.000, 34.000 -35.000 1.000, 28.879 -40.000 1.000, 28.900 -40.000 1.000, 28.414 -39.250 1.000, 29.000 -38.500 1.000, 28.279 -38.500 1.000, 27.829 -38.950 1.000, 28.879 -40.000 2.500, -16.000 -40.000 2.500, 6.440 -40.000 1.750, -15.502 -38.049 26.225, -16.351 -35.157 29.671, -16.143 -35.199 29.621, 7.773 -36.690 27.844, 31.898 -34.881 30.000, 28.541 -38.237 26.000, -16.236 -34.881 30.000, 31.029 -35.750 16.000, 33.779 -33.000 26.000, -20.000 -32.000 2.500, -21.000 -33.000 2.500, -21.000 -29.000 2.500, -22.071 -27.929 2.500, -23.750 -18.464 2.500, -21.000 -7.000 2.500, -26.500 -7.000 2.500, -26.500 -28.000 25.379, -26.500 -28.000 14.500, -26.500 -17.500 13.939, -26.500 -7.000 25.379, -26.223 -28.000 25.656, -26.026 -28.000 25.597, -26.310 -28.000 20.130, -26.282 -27.746 25.596, -25.641 -28.000 26.238, -23.592 -28.000 29.647, -20.382 -32.376 27.637, -15.678 -38.228 25.632, 33.900 -33.000 26.000, 34.000 -33.100 2.500, 34.400 -33.500 3.500, 34.900 -34.000 6.000, -21.000 -33.000 1.000, -16.000 -38.500 1.000, -16.000 -40.000 1.000, -16.000 -40.000 1.000, -17.000 -37.000 1.000, -15.500 -38.500 1.000, -17.828 -37.828 1.000, -21.000 -34.657 1.000, -21.000 -35.000 1.000, -15.000 -39.000 1.000, -16.000 -40.000 1.000, -21.000 -7.000 1.000, -21.000 -29.000 1.000, -20.000 -32.000 1.000, -21.000 -33.000 1.000, 34.000 -34.900 2.500, 29.000 -39.900 6.000, 29.000 -39.900 2.500, 31.950 -36.950 3.500, 34.000 -35.000 2.500, 29.000 -40.000 2.500, 29.000 -40.000 2.500, 28.890 -40.010 1.000, 28.879 -40.000 2.500, 28.884 -40.005 3.500, 28.890 -40.010 6.000, 28.879 -40.000 6.000, -15.858 -40.000 25.871, -15.858 -40.000 26.000, -16.000 -40.000 26.000, -16.000 -40.000 2.500, 28.879 -40.000 25.871, 6.440 -40.000 14.250, -16.000 -40.000 2.500, 33.779 -33.000 30.000, 32.440 -36.839 30.000, 32.079 -36.821 30.000, 29.958 -36.821 30.000, 34.000 -34.900 30.000, 34.000 -35.279 30.000, -14.259 -36.839 30.000, 8.882 -35.860 30.000, -19.829 -31.435 29.811, -23.426 -28.000 29.597, -23.308 -28.000 29.793, -23.515 -27.672 30.000, -19.868 -31.599 29.634, -26.500 -7.000 1.000, -24.598 -17.500 27.280, -22.697 -28.000 29.182, -22.697 -7.000 29.182, -28.000 -7.000 25.879, -27.939 -7.000 25.939, -26.939 -7.000 24.939, -27.250 -7.000 14.220, -28.000 -7.000 2.500, 33.900 -33.000 30.000, 34.900 -34.000 26.000, -21.000 -34.657 1.000, -17.828 -37.828 1.000, -21.000 -33.000 1.000, -23.000 -33.000 1.000, -22.657 -33.000 1.000, -16.061 -40.061 1.000, -28.000 -28.121 1.000, -28.000 -28.000 1.000, -28.000 -28.000 1.000, -22.030 -34.030 1.000, -26.500 -27.500 1.000, -22.071 -27.929 1.000, -24.071 -29.929 1.000, -23.750 -18.464 1.000, -21.000 -33.000 1.000, -21.000 -33.000 1.000, -21.000 -33.000 1.000, 29.000 -39.900 25.787, 29.000 -40.000 25.871, 29.000 -39.950 14.186, 31.500 -37.450 2.500, 29.871 -39.029 25.057, 31.950 -36.950 16.000, 30.663 -38.237 26.000, 28.900 -40.000 2.500, 28.900 -40.000 6.000, 28.895 -40.005 3.500, 28.884 -40.005 15.940, 28.890 -40.010 25.880, 29.101 -40.178 26.021, 28.900 -40.000 25.871, 30.250 -39.029 25.057, -15.680 -40.178 26.021, -16.132 -39.726 26.560, -15.929 -39.929 26.000, -16.000 -39.858 26.000, -16.649 -39.209 26.219, -16.649 -39.209 26.215, -18.000 -37.858 26.590, -17.820 -38.038 26.238, -18.000 -37.858 26.452, -17.795 -38.063 26.207, -17.620 -38.237 26.000, -16.000 -40.000 26.000, -15.929 -39.964 26.000, -16.000 -40.000 2.500, -16.030 -40.030 13.500, -16.061 -40.061 26.000, 34.000 -34.881 30.000, 34.000 -33.100 30.000, 34.000 -34.900 29.977, 34.000 -35.279 29.526, 31.979 -35.851 30.000, -17.748 -34.881 30.000, -15.745 -36.839 30.000, -16.004 -35.860 30.000, -14.167 -37.151 29.628, -14.552 -37.075 29.719, -16.207 -38.756 27.715, -16.770 -36.839 30.000, -16.980 -36.839 30.000, 7.730 -38.509 28.010, 31.550 -37.728 27.528, -23.106 -17.500 29.591, -23.515 -7.000 30.000, -24.788 -28.000 30.000, -24.000 -28.000 30.000, -24.000 -27.191 30.000, -25.133 -27.663 30.000, -24.337 -26.858 30.000, -17.748 -34.881 30.000, -20.684 -30.869 30.000, -16.236 -34.881 30.000, -23.515 -27.672 30.000, -23.515 -28.000 30.000, -28.000 -7.000 1.000, -24.000 -7.000 30.000, -25.404 -7.000 28.475, -25.464 -7.000 28.536, -25.318 -7.000 27.621, -26.500 -7.000 26.000, -27.879 -7.000 26.000, -27.000 -4.500 26.879, -28.000 -2.000 25.879, -27.000 -2.000 26.879, -28.000 -7.000 25.879, -27.939 -7.000 25.939, -27.879 -7.000 26.000, -25.404 -7.000 28.475, -25.879 -4.500 28.000, -25.404 -4.500 28.475, -28.000 -2.000 -0.500, -28.000 -8.000 1.000, -28.000 -8.000 -0.500, -28.000 -2.000 25.879, -28.000 -5.000 12.689, -27.220 -7.000 25.470, 34.000 -33.100 31.000, 34.900 -34.000 31.000, 34.400 -33.500 28.500, -26.500 -28.000 1.000, -24.899 -30.757 1.000, -25.328 -30.250 1.000, -28.000 -28.000 1.000, -28.000 -28.000 13.000, -28.000 -28.121 26.000, -28.000 -28.061 13.500, -28.000 -28.000 26.000, -27.250 -17.250 1.000, -28.000 -26.000 1.000, 28.950 -40.000 14.186, 34.000 -34.900 31.000, 28.895 -40.005 15.940, -16.915 -38.237 26.000, -27.388 -28.000 26.000, -21.818 -33.818 26.000, -15.964 -39.929 26.000, -18.000 -35.829 30.000, -18.000 -37.425 26.969, -18.000 -35.621 30.000, -16.650 -39.210 26.216, -27.991 -28.000 26.178, -21.756 -33.028 28.106, -25.694 -28.000 30.000, -27.967 -27.899 26.141, -27.900 -27.831 26.100, -27.860 -28.000 26.140, -27.943 -28.000 26.164, -22.162 -33.406 26.090, 34.000 -34.000 30.500, -18.000 -34.881 30.000, -13.691 -38.847 30.000, -12.969 -38.117 30.000, -15.020 -37.548 30.000, -13.668 -37.534 29.816, -13.078 -38.228 29.632, -14.049 -37.888 29.816, -15.532 -38.065 30.000, -23.757 -17.336 30.000, -24.000 -27.191 30.000, -24.464 -27.139 29.644, -24.406 -27.031 29.594, -24.356 -27.245 29.644, -24.457 -26.980 29.594, -25.133 -27.663 30.000, -24.788 -28.000 30.000, -24.263 -28.000 29.845, -24.337 -26.858 30.000, -18.000 -34.881 30.000, -18.000 -35.621 30.000, -25.694 -28.000 30.000, -23.515 -28.000 30.000, -24.000 -28.000 30.000, -25.464 -8.000 28.536, -26.491 -26.401 27.509, -26.000 -17.500 28.000, -28.000 -8.000 26.000, -25.464 -7.000 28.536, -26.464 -8.000 29.536, -25.464 -8.000 28.536, -25.915 -2.000 28.986, -26.464 -2.000 29.536, -25.915 -4.500 28.986, -25.934 -5.000 29.005, -25.500 -4.500 27.205, -25.500 -4.500 28.000, -27.000 -4.500 25.539, -27.000 -2.000 27.000, -27.000 11.979 26.270, -27.000 28.458 27.000, -27.000 28.458 25.539, -29.500 -2.000 26.000, -29.500 -2.000 25.500, -30.000 -2.000 26.000, -28.000 -2.000 26.000, -26.803 -2.000 28.000, -27.000 -2.000 28.000, -27.702 -2.000 27.237, -27.000 -2.000 27.781, -23.449 -4.500 30.604, -22.301 -4.500 33.000, -20.282 -4.500 33.000, -28.750 -2.000 12.689, -29.500 -2.000 -0.500, -29.500 -2.000 25.500, -28.939 -2.000 24.939, -28.000 -2.000 -0.500, -28.000 -8.000 -0.500, -29.500 -8.000 -0.500, -29.500 -2.000 -0.500, -29.500 -8.000 -0.500, -28.000 -8.000 2.500, -29.500 -8.000 25.500, -28.750 -8.000 12.689, -28.939 -8.000 24.939, -28.000 -8.000 25.879, -28.000 -28.000 2.500, -28.000 -26.000 13.000, -28.000 -27.000 7.000, -28.000 -26.000 2.500, -28.000 -18.000 14.250, -26.508 -28.000 28.393, -26.446 -26.355 27.662, -26.115 -27.178 27.993, -30.000 -8.000 26.000, -29.500 -8.000 26.000, -26.464 -8.000 29.536, -27.732 -8.000 27.268, -26.464 -2.000 29.536, -30.000 -2.000 26.000, -26.464 -8.000 29.536, -22.301 28.500 33.000, -24.552 12.000 30.500, -26.803 28.500 28.000, -26.250 -4.500 15.103, -25.500 -4.500 3.000, -27.000 -4.500 3.000, -27.000 28.059 27.607, -27.000 26.677 27.607, -27.000 28.458 27.781, -27.000 27.334 27.117, -27.000 13.229 27.391, -27.000 28.458 28.000, -27.000 28.458 33.000, -27.081 28.540 33.000, -27.000 28.458 27.781, -27.000 28.458 27.000, -27.000 28.458 25.539, -27.041 28.499 18.000, -27.081 28.540 3.000, -27.000 28.458 3.000, -27.000 -4.500 3.000, -27.000 -4.500 25.539, -27.000 28.458 28.000, -26.646 28.976 28.000, -27.000 28.621 28.000, -26.646 28.500 28.000, -30.000 -2.000 26.000, -28.750 -2.000 25.470, -29.500 -2.000 26.000, -28.000 -2.000 26.000, -20.866 28.500 33.000, -22.301 28.500 33.000, -22.144 27.220 33.000, -22.144 24.292 33.000, -22.301 -4.500 33.000, -20.282 -4.500 33.000, -21.292 12.000 33.000, -20.282 26.157 33.000, -25.500 28.000 27.205, -25.018 28.482 27.741, -22.891 11.991 30.103, -25.500 -4.500 27.205, -28.750 -8.000 25.470, -24.750 28.500 30.105, -26.803 28.500 28.000, -26.646 28.500 28.000, -25.500 11.750 15.103, -25.500 28.000 3.000, -25.500 9.500 3.000, -25.500 -4.500 3.000, -25.500 -4.500 1.500, -27.000 -4.500 1.500, -27.442 26.894 27.115, -27.000 28.059 27.607, -27.000 27.334 27.117, -27.000 26.677 27.607, -15.061 40.561 3.000, -15.000 40.500 3.000, -15.000 40.500 3.000, -15.000 40.500 3.000, -20.000 35.500 3.000, -27.000 28.500 3.000, -27.000 28.500 3.000, -22.000 33.500 3.000, -21.030 34.510 3.000, -15.121 40.500 28.000, -15.121 40.500 33.000, -15.061 40.561 33.000, -27.000 28.621 28.000, -26.646 28.976 28.000, -26.656 28.965 27.988, -27.000 28.621 33.000, -21.071 34.550 18.000, -24.921 30.700 28.000, -27.000 28.500 33.000, -27.000 28.540 30.500, -27.000 28.500 1.500, -27.000 12.000 2.250, -26.506 29.362 28.155, -20.282 28.500 33.000, -21.213 26.396 33.000, -14.456 33.516 31.868, -11.955 34.500 33.000, -18.254 31.718 30.375, -15.000 38.500 27.755, -15.000 36.751 29.056, -15.000 38.500 27.535, -20.000 33.500 15.377, -25.000 28.500 3.000, -20.000 33.500 3.000, -20.000 33.500 3.000, -15.000 38.500 3.000, -14.500 39.000 3.000, -14.500 39.000 27.101, -14.877 34.500 33.000, -15.000 34.452 32.945, -15.000 35.228 32.367, -14.780 34.598 33.000, -15.708 40.118 28.201, -19.492 33.109 30.596, -21.457 28.500 3.000, -22.750 19.728 3.000, -20.000 9.500 3.000, -20.000 29.957 3.000, -25.500 9.500 1.500, -20.000 9.500 1.500, -21.457 28.500 1.500, -20.000 29.957 1.500, -25.500 -4.500 1.500, -27.000 -4.500 1.500, -25.500 9.500 1.500, -27.000 28.500 1.500, -23.500 12.728 1.500, -15.000 40.500 33.000, -15.000 40.500 28.000, -15.000 40.500 27.784, -15.000 40.500 25.797, -15.030 40.530 18.000, -27.000 28.500 1.500, -15.000 40.500 1.500, -20.000 35.500 1.500, -22.000 33.500 1.500, -21.000 34.500 2.250, -15.000 40.500 1.500, 27.929 40.500 3.000, 27.929 40.500 1.500, 6.464 40.500 2.250, -20.976 34.719 28.077, -15.173 40.350 28.000, -15.000 40.500 33.000, -15.061 40.500 30.500, -12.825 40.350 28.000, 28.000 40.500 28.000, 28.000 40.252 28.000, 28.000 40.118 28.000, 28.281 40.124 28.000, 33.000 35.500 28.000, 33.000 35.331 28.000, -12.922 40.252 28.000, 24.682 34.275 32.741, 27.560 34.319 32.792, 28.000 34.500 33.000, 28.000 34.151 32.599, 6.772 34.008 32.434, 26.673 33.516 31.868, 27.379 39.000 27.101, 27.651 38.728 27.337, 6.325 36.258 29.484, -20.000 33.500 3.000, -16.457 33.500 3.000, -15.000 34.957 3.000, 32.879 33.500 3.000, 8.211 36.250 3.000, 27.379 39.000 3.000, -13.367 34.549 33.000, -11.857 34.598 33.000, -13.783 37.474 30.500, -15.000 34.500 33.000, -20.000 9.500 1.500, -20.000 9.500 3.000, -20.000 29.957 3.000, -27.000 28.500 1.500, -22.000 33.500 1.500, -20.000 33.500 1.500, -14.825 40.500 27.869, -14.698 40.500 27.784, -12.850 40.500 27.869, 6.500 40.500 27.892, 28.000 40.500 27.784, -12.964 40.500 27.784, -13.783 40.500 27.173, 27.416 40.500 27.658, 25.442 40.500 27.623, 26.387 40.500 26.944, 28.000 40.500 25.797, 6.500 40.500 26.791, 28.000 40.500 3.000, 6.500 40.500 14.398, -20.000 35.500 1.500, -16.457 33.500 1.500, -15.000 34.957 1.500, 27.964 40.464 3.000, 28.000 40.429 3.000, 33.000 35.429 3.000, 33.000 35.429 1.500, 30.464 37.964 2.250, 33.000 33.500 1.500, 27.929 40.500 1.500, 33.000 35.429 1.500, 8.272 37.000 1.500, 30.642 37.729 28.003, 28.294 40.118 28.005, 33.000 35.429 28.067, 33.000 33.500 27.334, 33.000 33.500 28.000, 33.000 33.379 28.000, 33.000 33.912 27.031, 33.000 34.439 15.500, 33.000 35.500 3.000, 33.000 33.500 3.000, 33.000 33.379 3.000, 30.500 38.000 15.500, -12.390 37.425 30.500, 7.539 37.376 30.500, 28.000 38.516 29.393, 26.529 33.909 32.774, 28.377 33.500 32.806, 25.455 33.500 32.755, 29.484 35.664 30.421, 33.000 33.500 29.484, 32.879 33.500 27.421, 28.398 34.276 30.070, 32.879 33.500 28.000, 30.189 36.189 15.500, -20.000 33.500 3.000, -16.457 33.500 3.000, 33.000 33.500 1.500, 8.272 33.500 2.250, -16.457 33.500 1.500, -13.837 40.500 27.827, -13.831 40.500 27.479, 26.429 40.500 27.301, 27.964 40.482 3.000, 30.500 37.964 3.000, 32.970 33.470 3.000 ] } colorPerVertex FALSE coordIndex [ 0, 1, 2, -1, 0, 2, 3, -1, 4, 5, 6, -1, 5, 4, 7, -1, 7, 4, 8, -1, 4, 6, 9, -1, 4, 9, 10, -1, 8, 4, 11, -1, 11, 4, 12, -1, 4, 10, 13, -1, 4, 13, 14, -1, 12, 4, 15, -1, 15, 4, 16, -1, 4, 14, 17, -1, 18, 4, 19, -1, 4, 17, 19, -1, 18, 19, 20, -1, 18, 20, 21, -1, 18, 21, 22, -1, 18, 22, 23, -1, 24, 16, 25, -1, 26, 27, 25, -1, 27, 28, 25, -1, 28, 24, 25, -1, 16, 4, 25, -1, 29, 26, 30, -1, 26, 25, 30, -1, 29, 30, 31, -1, 32, 33, 31, -1, 33, 34, 31, -1, 34, 35, 31, -1, 35, 36, 31, -1, 36, 29, 31, -1, 32, 31, 37, -1, 38, 39, 37, -1, 39, 40, 37, -1, 40, 41, 37, -1, 41, 42, 37, -1, 42, 43, 37, -1, 43, 44, 37, -1, 44, 45, 37, -1, 45, 46, 37, -1, 46, 47, 37, -1, 47, 48, 37, -1, 48, 49, 37, -1, 49, 32, 37, -1, 50, 51, 2, -1, 51, 38, 2, -1, 1, 52, 2, -1, 38, 37, 2, -1, 18, 23, 53, -1, 23, 54, 53, -1, 54, 55, 53, -1, 55, 56, 53, -1, 56, 57, 53, -1, 57, 58, 53, -1, 58, 59, 53, -1, 59, 60, 53, -1, 60, 61, 53, -1, 61, 62, 53, -1, 62, 63, 53, -1, 63, 64, 53, -1, 64, 65, 53, -1, 65, 66, 53, -1, 66, 67, 53, -1, 67, 50, 53, -1, 52, 18, 53, -1, 2, 52, 53, -1, 50, 2, 53, -1, 0, 68, 52, -1, 0, 52, 1, -1, 69, 70, 71, -1, 72, 71, 70, -1, 73, 69, 71, -1, 74, 71, 72, -1, 75, 73, 71, -1, 76, 71, 74, -1, 77, 75, 71, -1, 78, 71, 76, -1, 79, 71, 80, -1, 79, 77, 71, -1, 81, 71, 78, -1, 82, 79, 80, -1, 83, 71, 81, -1, 84, 82, 80, -1, 85, 84, 80, -1, 86, 85, 80, -1, 87, 86, 80, -1, 88, 87, 80, -1, 89, 88, 80, -1, 90, 89, 80, -1, 91, 90, 80, -1, 92, 83, 93, -1, 92, 94, 95, -1, 92, 96, 94, -1, 92, 97, 96, -1, 92, 98, 97, -1, 92, 93, 98, -1, 92, 71, 83, -1, 99, 92, 95, -1, 99, 95, 100, -1, 68, 101, 91, -1, 68, 91, 80, -1, 102, 101, 68, -1, 103, 99, 100, -1, 103, 104, 105, -1, 103, 106, 104, -1, 103, 107, 106, -1, 103, 100, 107, -1, 108, 103, 105, -1, 109, 110, 111, -1, 109, 112, 110, -1, 109, 113, 112, -1, 109, 114, 113, -1, 109, 108, 114, -1, 109, 103, 108, -1, 115, 109, 111, -1, 116, 109, 115, -1, 117, 109, 116, -1, 3, 118, 119, -1, 3, 120, 118, -1, 3, 121, 120, -1, 3, 122, 121, -1, 3, 123, 122, -1, 3, 124, 123, -1, 3, 125, 124, -1, 3, 126, 125, -1, 3, 127, 126, -1, 3, 117, 127, -1, 3, 68, 0, -1, 3, 109, 117, -1, 128, 129, 102, -1, 128, 130, 129, -1, 128, 102, 68, -1, 128, 131, 130, -1, 128, 132, 131, -1, 128, 119, 132, -1, 128, 68, 3, -1, 128, 3, 119, -1, 37, 109, 3, -1, 2, 37, 3, -1, 133, 24, 134, -1, 16, 24, 133, -1, 15, 133, 135, -1, 15, 16, 133, -1, 12, 135, 136, -1, 12, 15, 135, -1, 11, 136, 137, -1, 11, 12, 136, -1, 138, 11, 137, -1, 8, 11, 138, -1, 7, 138, 139, -1, 7, 8, 138, -1, 5, 139, 140, -1, 5, 7, 139, -1, 6, 140, 141, -1, 6, 5, 140, -1, 142, 6, 141, -1, 9, 6, 142, -1, 143, 9, 142, -1, 10, 9, 143, -1, 144, 10, 143, -1, 13, 10, 144, -1, 14, 144, 145, -1, 14, 13, 144, -1, 146, 14, 145, -1, 17, 14, 146, -1, 147, 17, 146, -1, 19, 17, 147, -1, 20, 147, 148, -1, 20, 19, 147, -1, 149, 20, 148, -1, 21, 20, 149, -1, 150, 21, 149, -1, 22, 21, 150, -1, 23, 150, 151, -1, 23, 22, 150, -1, 152, 23, 151, -1, 54, 23, 152, -1, 153, 54, 152, -1, 55, 54, 153, -1, 56, 55, 153, -1, 56, 153, 154, -1, 155, 56, 154, -1, 57, 56, 155, -1, 156, 57, 155, -1, 58, 57, 156, -1, 157, 58, 156, -1, 59, 58, 157, -1, 60, 59, 157, -1, 60, 157, 158, -1, 159, 60, 158, -1, 61, 60, 159, -1, 62, 61, 159, -1, 62, 159, 160, -1, 63, 62, 160, -1, 63, 160, 161, -1, 64, 161, 162, -1, 64, 63, 161, -1, 163, 64, 162, -1, 65, 64, 163, -1, 66, 163, 164, -1, 66, 65, 163, -1, 67, 164, 165, -1, 67, 66, 164, -1, 166, 67, 165, -1, 50, 67, 166, -1, 51, 166, 167, -1, 51, 50, 166, -1, 168, 51, 167, -1, 38, 51, 168, -1, 39, 168, 169, -1, 39, 38, 168, -1, 170, 39, 169, -1, 40, 39, 170, -1, 41, 170, 171, -1, 41, 40, 170, -1, 42, 171, 172, -1, 42, 41, 171, -1, 173, 42, 172, -1, 43, 42, 173, -1, 44, 173, 174, -1, 44, 43, 173, -1, 45, 174, 175, -1, 45, 44, 174, -1, 176, 45, 175, -1, 46, 45, 176, -1, 47, 176, 177, -1, 47, 46, 176, -1, 178, 47, 177, -1, 48, 47, 178, -1, 49, 178, 179, -1, 49, 48, 178, -1, 32, 49, 179, -1, 32, 179, 180, -1, 33, 32, 180, -1, 33, 180, 181, -1, 34, 33, 181, -1, 34, 181, 182, -1, 35, 34, 182, -1, 35, 182, 183, -1, 184, 35, 183, -1, 36, 35, 184, -1, 29, 36, 184, -1, 29, 184, 185, -1, 26, 29, 185, -1, 26, 185, 186, -1, 27, 26, 186, -1, 27, 186, 187, -1, 188, 27, 187, -1, 28, 27, 188, -1, 134, 28, 188, -1, 24, 28, 134, -1, 30, 25, 189, -1, 25, 190, 189, -1, 4, 191, 25, -1, 4, 192, 191, -1, 30, 189, 193, -1, 31, 30, 193, -1, 194, 195, 196, -1, 197, 194, 196, -1, 198, 197, 196, -1, 196, 199, 200, -1, 199, 201, 200, -1, 201, 202, 200, -1, 202, 203, 200, -1, 203, 198, 200, -1, 198, 196, 200, -1, 31, 193, 204, -1, 37, 31, 204, -1, 205, 18, 52, -1, 206, 205, 52, -1, 207, 52, 68, -1, 207, 206, 52, -1, 208, 93, 83, -1, 208, 209, 93, -1, 210, 83, 81, -1, 210, 208, 83, -1, 211, 81, 78, -1, 211, 210, 81, -1, 212, 78, 76, -1, 212, 211, 78, -1, 74, 212, 76, -1, 213, 212, 74, -1, 214, 74, 72, -1, 214, 213, 74, -1, 70, 214, 72, -1, 215, 214, 70, -1, 216, 70, 69, -1, 216, 215, 70, -1, 73, 216, 69, -1, 217, 216, 73, -1, 218, 73, 75, -1, 218, 217, 73, -1, 219, 75, 77, -1, 219, 218, 75, -1, 220, 77, 79, -1, 220, 219, 77, -1, 221, 79, 82, -1, 221, 220, 79, -1, 222, 82, 84, -1, 222, 221, 82, -1, 223, 84, 85, -1, 223, 222, 84, -1, 224, 85, 86, -1, 224, 223, 85, -1, 225, 86, 87, -1, 225, 224, 86, -1, 226, 87, 88, -1, 226, 225, 87, -1, 227, 88, 89, -1, 227, 226, 88, -1, 228, 89, 90, -1, 228, 227, 89, -1, 229, 228, 90, -1, 229, 90, 91, -1, 101, 229, 91, -1, 230, 229, 101, -1, 231, 230, 101, -1, 231, 101, 102, -1, 129, 231, 102, -1, 232, 231, 129, -1, 233, 232, 129, -1, 233, 129, 130, -1, 131, 233, 130, -1, 234, 233, 131, -1, 235, 234, 131, -1, 235, 131, 132, -1, 119, 235, 132, -1, 236, 235, 119, -1, 118, 236, 119, -1, 237, 236, 118, -1, 238, 118, 120, -1, 238, 237, 118, -1, 121, 238, 120, -1, 239, 238, 121, -1, 240, 121, 122, -1, 240, 239, 121, -1, 123, 240, 122, -1, 241, 240, 123, -1, 242, 123, 124, -1, 242, 241, 123, -1, 125, 242, 124, -1, 243, 242, 125, -1, 244, 125, 126, -1, 244, 243, 125, -1, 245, 126, 127, -1, 245, 244, 126, -1, 117, 245, 127, -1, 246, 245, 117, -1, 247, 117, 116, -1, 247, 246, 117, -1, 115, 247, 116, -1, 248, 247, 115, -1, 249, 115, 111, -1, 249, 248, 115, -1, 250, 111, 110, -1, 250, 249, 111, -1, 112, 250, 110, -1, 251, 250, 112, -1, 252, 112, 113, -1, 252, 251, 112, -1, 114, 252, 113, -1, 253, 252, 114, -1, 108, 253, 114, -1, 254, 253, 108, -1, 105, 254, 108, -1, 255, 254, 105, -1, 104, 255, 105, -1, 256, 255, 104, -1, 106, 256, 104, -1, 257, 256, 106, -1, 258, 257, 106, -1, 258, 106, 107, -1, 100, 258, 107, -1, 259, 258, 100, -1, 95, 259, 100, -1, 260, 259, 95, -1, 94, 260, 95, -1, 261, 260, 94, -1, 262, 261, 94, -1, 262, 94, 96, -1, 97, 262, 96, -1, 263, 262, 97, -1, 98, 263, 97, -1, 264, 263, 98, -1, 209, 264, 98, -1, 209, 98, 93, -1, 265, 92, 99, -1, 265, 266, 92, -1, 267, 92, 268, -1, 267, 71, 92, -1, 103, 265, 99, -1, 269, 265, 103, -1, 270, 271, 272, -1, 71, 267, 80, -1, 272, 273, 274, -1, 273, 80, 274, -1, 267, 270, 274, -1, 270, 272, 274, -1, 80, 267, 274, -1, 275, 269, 103, -1, 275, 103, 109, -1, 68, 80, 276, -1, 68, 276, 207, -1, 109, 37, 204, -1, 109, 204, 275, -1, 277, 134, 278, -1, 279, 277, 278, -1, 280, 279, 278, -1, 281, 280, 278, -1, 282, 281, 278, -1, 283, 282, 278, -1, 284, 283, 278, -1, 285, 284, 278, -1, 286, 285, 278, -1, 287, 286, 278, -1, 288, 287, 278, -1, 289, 288, 278, -1, 290, 289, 278, -1, 147, 290, 278, -1, 291, 147, 278, -1, 292, 291, 278, -1, 293, 292, 278, -1, 294, 293, 278, -1, 295, 294, 278, -1, 296, 295, 278, -1, 297, 296, 278, -1, 298, 297, 278, -1, 299, 298, 278, -1, 300, 299, 278, -1, 301, 300, 278, -1, 302, 301, 278, -1, 303, 302, 278, -1, 161, 303, 278, -1, 304, 161, 278, -1, 305, 304, 278, -1, 306, 305, 278, -1, 307, 306, 278, -1, 308, 307, 278, -1, 309, 308, 278, -1, 310, 309, 278, -1, 311, 310, 278, -1, 312, 311, 278, -1, 313, 312, 278, -1, 314, 313, 278, -1, 315, 314, 278, -1, 316, 315, 278, -1, 317, 316, 278, -1, 318, 317, 278, -1, 319, 318, 278, -1, 320, 319, 278, -1, 321, 320, 278, -1, 180, 321, 278, -1, 181, 180, 278, -1, 182, 181, 278, -1, 183, 182, 278, -1, 322, 183, 278, -1, 185, 322, 278, -1, 323, 185, 278, -1, 324, 323, 278, -1, 325, 324, 278, -1, 134, 325, 278, -1, 191, 326, 190, -1, 25, 191, 190, -1, 189, 190, 327, -1, 193, 327, 204, -1, 266, 265, 328, -1, 328, 269, 275, -1, 190, 266, 329, -1, 275, 204, 329, -1, 204, 327, 329, -1, 327, 190, 329, -1, 266, 328, 329, -1, 328, 275, 329, -1, 191, 330, 326, -1, 191, 331, 330, -1, 332, 192, 333, -1, 332, 191, 192, -1, 332, 333, 331, -1, 332, 331, 191, -1, 193, 189, 327, -1, 333, 334, 335, -1, 334, 336, 335, -1, 336, 337, 335, -1, 337, 331, 335, -1, 331, 333, 335, -1, 338, 339, 340, -1, 339, 341, 340, -1, 341, 342, 340, -1, 342, 343, 340, -1, 343, 344, 340, -1, 344, 345, 340, -1, 345, 346, 340, -1, 346, 347, 340, -1, 348, 349, 340, -1, 347, 350, 340, -1, 351, 352, 340, -1, 353, 354, 340, -1, 354, 355, 340, -1, 355, 356, 340, -1, 357, 358, 348, -1, 356, 359, 340, -1, 334, 357, 348, -1, 359, 360, 340, -1, 360, 361, 340, -1, 361, 362, 340, -1, 362, 363, 340, -1, 363, 364, 340, -1, 364, 365, 340, -1, 365, 366, 340, -1, 366, 367, 340, -1, 367, 351, 340, -1, 350, 353, 340, -1, 358, 368, 369, -1, 368, 358, 370, -1, 358, 369, 371, -1, 370, 358, 372, -1, 358, 371, 373, -1, 372, 358, 374, -1, 358, 373, 375, -1, 374, 358, 376, -1, 358, 375, 377, -1, 376, 358, 378, -1, 358, 377, 379, -1, 378, 358, 380, -1, 348, 358, 381, -1, 358, 379, 381, -1, 348, 381, 382, -1, 348, 382, 383, -1, 348, 383, 384, -1, 348, 384, 385, -1, 348, 385, 386, -1, 387, 388, 389, -1, 388, 390, 389, -1, 390, 391, 389, -1, 391, 392, 389, -1, 392, 380, 389, -1, 380, 358, 389, -1, 387, 389, 393, -1, 348, 386, 349, -1, 393, 389, 394, -1, 395, 396, 397, -1, 396, 398, 397, -1, 398, 394, 397, -1, 394, 389, 397, -1, 395, 397, 399, -1, 399, 397, 400, -1, 400, 397, 401, -1, 401, 397, 402, -1, 403, 402, 404, -1, 402, 397, 404, -1, 405, 406, 404, -1, 406, 407, 404, -1, 407, 408, 404, -1, 408, 409, 404, -1, 409, 403, 404, -1, 271, 352, 272, -1, 352, 351, 410, -1, 352, 410, 411, -1, 352, 411, 412, -1, 352, 412, 413, -1, 352, 413, 414, -1, 352, 414, 415, -1, 272, 352, 416, -1, 415, 417, 416, -1, 352, 415, 416, -1, 417, 418, 416, -1, 416, 418, 419, -1, 350, 420, 353, -1, 416, 419, 421, -1, 420, 422, 423, -1, 353, 420, 423, -1, 416, 421, 424, -1, 423, 422, 425, -1, 422, 426, 425, -1, 416, 424, 427, -1, 425, 426, 428, -1, 426, 429, 428, -1, 416, 427, 430, -1, 428, 429, 431, -1, 416, 430, 432, -1, 416, 432, 433, -1, 416, 433, 434, -1, 416, 434, 435, -1, 416, 435, 436, -1, 416, 436, 437, -1, 438, 439, 440, -1, 439, 441, 440, -1, 441, 442, 440, -1, 442, 443, 440, -1, 443, 444, 440, -1, 444, 445, 440, -1, 446, 447, 448, -1, 447, 449, 448, -1, 449, 450, 448, -1, 450, 451, 448, -1, 451, 452, 448, -1, 452, 453, 448, -1, 453, 438, 448, -1, 416, 437, 448, -1, 438, 440, 448, -1, 437, 446, 448, -1, 445, 454, 455, -1, 431, 429, 455, -1, 405, 404, 455, -1, 429, 405, 455, -1, 440, 445, 455, -1, 454, 456, 455, -1, 456, 457, 455, -1, 457, 458, 455, -1, 458, 459, 455, -1, 459, 460, 455, -1, 460, 431, 455, -1, 352, 348, 340, -1, 349, 461, 340, -1, 461, 462, 340, -1, 462, 463, 340, -1, 463, 338, 340, -1, 464, 465, 197, -1, 465, 194, 197, -1, 205, 466, 467, -1, 205, 467, 18, -1, 198, 468, 469, -1, 197, 198, 469, -1, 470, 471, 472, -1, 472, 471, 203, -1, 471, 473, 474, -1, 473, 198, 474, -1, 198, 203, 474, -1, 203, 471, 474, -1, 472, 475, 470, -1, 205, 475, 472, -1, 476, 205, 206, -1, 476, 477, 205, -1, 478, 479, 477, -1, 480, 479, 404, -1, 480, 404, 475, -1, 480, 475, 205, -1, 480, 477, 479, -1, 480, 205, 477, -1, 481, 482, 483, -1, 484, 482, 481, -1, 484, 485, 482, -1, 486, 481, 483, -1, 486, 483, 487, -1, 488, 485, 484, -1, 488, 489, 490, -1, 488, 490, 485, -1, 491, 489, 488, -1, 491, 492, 489, -1, 493, 486, 487, -1, 493, 487, 494, -1, 495, 493, 494, -1, 495, 494, 496, -1, 497, 492, 491, -1, 497, 498, 492, -1, 499, 495, 496, -1, 499, 496, 500, -1, 501, 498, 497, -1, 501, 502, 498, -1, 503, 499, 500, -1, 503, 500, 504, -1, 505, 506, 502, -1, 505, 502, 501, -1, 507, 508, 506, -1, 507, 506, 505, -1, 509, 503, 504, -1, 509, 504, 510, -1, 511, 512, 508, -1, 511, 508, 507, -1, 513, 509, 510, -1, 513, 510, 514, -1, 515, 513, 514, -1, 515, 514, 516, -1, 517, 512, 511, -1, 517, 518, 512, -1, 519, 515, 516, -1, 519, 516, 520, -1, 519, 520, 521, -1, 522, 518, 517, -1, 522, 523, 518, -1, 524, 519, 521, -1, 524, 521, 525, -1, 526, 523, 522, -1, 526, 527, 523, -1, 528, 525, 529, -1, 528, 524, 525, -1, 530, 527, 526, -1, 530, 531, 532, -1, 530, 532, 527, -1, 533, 528, 529, -1, 534, 531, 530, -1, 535, 533, 529, -1, 535, 536, 533, -1, 537, 531, 534, -1, 537, 534, 538, -1, 539, 536, 535, -1, 539, 540, 536, -1, 541, 538, 542, -1, 541, 537, 538, -1, 543, 540, 539, -1, 543, 544, 540, -1, 545, 542, 546, -1, 545, 541, 542, -1, 261, 544, 543, -1, 261, 547, 544, -1, 548, 546, 549, -1, 548, 545, 546, -1, 262, 547, 261, -1, 262, 550, 547, -1, 551, 549, 552, -1, 551, 548, 549, -1, 553, 554, 550, -1, 553, 550, 262, -1, 555, 552, 556, -1, 555, 551, 552, -1, 557, 558, 554, -1, 557, 554, 553, -1, 559, 556, 560, -1, 559, 555, 556, -1, 561, 558, 557, -1, 562, 560, 563, -1, 562, 559, 560, -1, 564, 565, 558, -1, 564, 558, 561, -1, 566, 563, 567, -1, 566, 562, 563, -1, 568, 569, 565, -1, 568, 565, 564, -1, 570, 566, 567, -1, 570, 567, 571, -1, 572, 573, 569, -1, 572, 569, 568, -1, 574, 570, 571, -1, 575, 576, 573, -1, 575, 573, 572, -1, 577, 574, 571, -1, 577, 571, 578, -1, 579, 576, 575, -1, 579, 580, 576, -1, 581, 578, 582, -1, 581, 577, 578, -1, 583, 584, 580, -1, 583, 580, 579, -1, 585, 582, 586, -1, 585, 581, 582, -1, 587, 586, 584, -1, 587, 584, 583, -1, 587, 585, 586, -1, 92, 266, 588, -1, 268, 92, 588, -1, 268, 588, 589, -1, 590, 268, 589, -1, 591, 270, 267, -1, 591, 590, 270, -1, 591, 267, 268, -1, 591, 268, 590, -1, 328, 265, 269, -1, 272, 416, 592, -1, 273, 272, 592, -1, 593, 594, 273, -1, 593, 595, 594, -1, 593, 592, 595, -1, 593, 273, 592, -1, 596, 276, 597, -1, 598, 276, 80, -1, 598, 80, 273, -1, 598, 594, 597, -1, 598, 273, 594, -1, 598, 597, 276, -1, 599, 271, 270, -1, 599, 600, 271, -1, 599, 601, 600, -1, 599, 590, 601, -1, 599, 270, 590, -1, 326, 330, 190, -1, 266, 589, 588, -1, 330, 589, 602, -1, 266, 190, 602, -1, 190, 330, 602, -1, 589, 266, 602, -1, 337, 601, 603, -1, 601, 590, 603, -1, 590, 589, 603, -1, 589, 330, 603, -1, 330, 331, 603, -1, 331, 337, 603, -1, 336, 334, 600, -1, 600, 334, 271, -1, 600, 337, 336, -1, 601, 337, 600, -1, 402, 604, 605, -1, 402, 403, 604, -1, 401, 605, 606, -1, 401, 402, 605, -1, 400, 606, 607, -1, 400, 401, 606, -1, 399, 607, 608, -1, 399, 400, 607, -1, 609, 399, 608, -1, 395, 399, 609, -1, 396, 609, 610, -1, 396, 395, 609, -1, 611, 396, 610, -1, 398, 396, 611, -1, 394, 611, 612, -1, 394, 398, 611, -1, 613, 394, 612, -1, 393, 394, 613, -1, 387, 613, 614, -1, 387, 393, 613, -1, 388, 614, 615, -1, 388, 387, 614, -1, 390, 615, 616, -1, 390, 388, 615, -1, 391, 616, 617, -1, 391, 390, 616, -1, 392, 617, 618, -1, 392, 391, 617, -1, 380, 618, 619, -1, 380, 392, 618, -1, 378, 619, 620, -1, 378, 380, 619, -1, 376, 620, 621, -1, 376, 378, 620, -1, 374, 621, 622, -1, 374, 376, 621, -1, 372, 622, 623, -1, 372, 374, 622, -1, 370, 623, 624, -1, 370, 372, 623, -1, 368, 370, 624, -1, 368, 624, 625, -1, 626, 368, 625, -1, 369, 368, 626, -1, 371, 369, 626, -1, 371, 626, 627, -1, 628, 371, 627, -1, 373, 371, 628, -1, 375, 373, 628, -1, 375, 628, 629, -1, 630, 375, 629, -1, 377, 375, 630, -1, 379, 377, 630, -1, 379, 630, 631, -1, 632, 379, 631, -1, 381, 379, 632, -1, 633, 381, 632, -1, 382, 381, 633, -1, 383, 633, 634, -1, 383, 382, 633, -1, 635, 383, 634, -1, 384, 383, 635, -1, 385, 635, 636, -1, 385, 384, 635, -1, 637, 385, 636, -1, 386, 385, 637, -1, 349, 637, 638, -1, 349, 386, 637, -1, 639, 349, 638, -1, 461, 349, 639, -1, 462, 639, 640, -1, 462, 461, 639, -1, 463, 640, 641, -1, 463, 462, 640, -1, 642, 463, 641, -1, 338, 463, 642, -1, 339, 642, 643, -1, 339, 338, 642, -1, 644, 339, 643, -1, 341, 339, 644, -1, 342, 644, 645, -1, 342, 341, 644, -1, 343, 645, 646, -1, 343, 342, 645, -1, 647, 343, 646, -1, 344, 343, 647, -1, 345, 647, 648, -1, 345, 344, 647, -1, 649, 345, 648, -1, 346, 345, 649, -1, 650, 346, 649, -1, 347, 346, 650, -1, 651, 347, 650, -1, 350, 347, 651, -1, 652, 350, 651, -1, 420, 350, 652, -1, 653, 420, 652, -1, 422, 420, 653, -1, 426, 422, 653, -1, 426, 653, 654, -1, 655, 426, 654, -1, 429, 426, 655, -1, 656, 429, 655, -1, 405, 429, 656, -1, 657, 405, 656, -1, 406, 405, 657, -1, 407, 406, 657, -1, 407, 657, 658, -1, 659, 407, 658, -1, 408, 407, 659, -1, 660, 408, 659, -1, 409, 408, 660, -1, 403, 409, 660, -1, 403, 660, 604, -1, 456, 661, 662, -1, 456, 454, 661, -1, 457, 662, 663, -1, 457, 456, 662, -1, 458, 663, 664, -1, 458, 457, 663, -1, 459, 664, 665, -1, 459, 458, 664, -1, 666, 459, 665, -1, 460, 459, 666, -1, 431, 666, 667, -1, 431, 460, 666, -1, 668, 431, 667, -1, 428, 431, 668, -1, 425, 668, 669, -1, 425, 428, 668, -1, 670, 425, 669, -1, 423, 425, 670, -1, 353, 670, 671, -1, 353, 423, 670, -1, 354, 671, 672, -1, 354, 353, 671, -1, 355, 672, 673, -1, 355, 354, 672, -1, 356, 673, 674, -1, 356, 355, 673, -1, 359, 674, 675, -1, 359, 356, 674, -1, 360, 675, 676, -1, 360, 359, 675, -1, 361, 676, 677, -1, 361, 360, 676, -1, 362, 677, 678, -1, 362, 361, 677, -1, 363, 678, 679, -1, 363, 362, 678, -1, 364, 679, 680, -1, 364, 363, 679, -1, 365, 680, 681, -1, 365, 364, 680, -1, 366, 365, 681, -1, 366, 681, 682, -1, 683, 366, 682, -1, 367, 366, 683, -1, 351, 367, 683, -1, 351, 683, 684, -1, 685, 351, 684, -1, 410, 351, 685, -1, 411, 410, 685, -1, 411, 685, 686, -1, 687, 411, 686, -1, 412, 411, 687, -1, 413, 412, 687, -1, 413, 687, 688, -1, 689, 413, 688, -1, 414, 413, 689, -1, 690, 414, 689, -1, 415, 414, 690, -1, 417, 690, 691, -1, 417, 415, 690, -1, 692, 417, 691, -1, 418, 417, 692, -1, 419, 692, 693, -1, 419, 418, 692, -1, 694, 419, 693, -1, 421, 419, 694, -1, 424, 694, 695, -1, 424, 421, 694, -1, 696, 424, 695, -1, 427, 424, 696, -1, 430, 696, 697, -1, 430, 427, 696, -1, 432, 697, 698, -1, 432, 430, 697, -1, 699, 432, 698, -1, 433, 432, 699, -1, 434, 699, 700, -1, 434, 433, 699, -1, 701, 434, 700, -1, 435, 434, 701, -1, 436, 701, 702, -1, 436, 435, 701, -1, 437, 702, 703, -1, 437, 436, 702, -1, 704, 437, 703, -1, 446, 437, 704, -1, 447, 704, 705, -1, 447, 446, 704, -1, 706, 447, 705, -1, 449, 447, 706, -1, 707, 449, 706, -1, 450, 449, 707, -1, 708, 450, 707, -1, 451, 450, 708, -1, 709, 451, 708, -1, 452, 451, 709, -1, 710, 452, 709, -1, 453, 452, 710, -1, 438, 453, 710, -1, 438, 710, 711, -1, 712, 438, 711, -1, 439, 438, 712, -1, 713, 439, 712, -1, 441, 439, 713, -1, 714, 441, 713, -1, 442, 441, 714, -1, 443, 442, 714, -1, 443, 714, 715, -1, 716, 443, 715, -1, 444, 443, 716, -1, 717, 444, 716, -1, 445, 444, 717, -1, 454, 445, 717, -1, 454, 717, 661, -1, 718, 719, 720, -1, 358, 718, 720, -1, 719, 721, 720, -1, 389, 358, 720, -1, 721, 389, 720, -1, 334, 348, 352, -1, 271, 334, 352, -1, 722, 723, 724, -1, 722, 724, 473, -1, 725, 726, 722, -1, 725, 471, 726, -1, 725, 473, 471, -1, 725, 722, 473, -1, 404, 397, 727, -1, 475, 404, 727, -1, 596, 440, 455, -1, 597, 440, 596, -1, 595, 448, 440, -1, 728, 440, 597, -1, 728, 597, 594, -1, 728, 594, 595, -1, 728, 595, 440, -1, 729, 730, 731, -1, 729, 731, 416, -1, 729, 732, 730, -1, 729, 416, 448, -1, 729, 448, 732, -1, 197, 469, 733, -1, 734, 735, 733, -1, 736, 464, 197, -1, 736, 735, 464, -1, 736, 197, 733, -1, 736, 733, 735, -1, 735, 737, 465, -1, 735, 465, 464, -1, 738, 468, 198, -1, 739, 473, 740, -1, 739, 740, 741, -1, 740, 473, 742, -1, 739, 738, 743, -1, 198, 473, 743, -1, 738, 198, 743, -1, 473, 739, 743, -1, 744, 745, 746, -1, 744, 746, 747, -1, 748, 749, 475, -1, 727, 748, 475, -1, 750, 558, 565, -1, 750, 565, 569, -1, 750, 569, 573, -1, 750, 573, 576, -1, 750, 576, 580, -1, 750, 580, 584, -1, 750, 584, 586, -1, 750, 586, 582, -1, 750, 582, 578, -1, 750, 578, 571, -1, 750, 571, 567, -1, 750, 567, 563, -1, 750, 563, 560, -1, 750, 560, 556, -1, 750, 556, 552, -1, 750, 552, 549, -1, 750, 549, 546, -1, 750, 546, 542, -1, 750, 542, 538, -1, 750, 538, 534, -1, 750, 534, 530, -1, 750, 530, 526, -1, 750, 526, 522, -1, 750, 522, 517, -1, 750, 517, 511, -1, 750, 511, 507, -1, 750, 507, 505, -1, 750, 505, 501, -1, 750, 501, 497, -1, 750, 497, 491, -1, 750, 491, 488, -1, 750, 488, 484, -1, 750, 484, 481, -1, 750, 481, 486, -1, 750, 486, 493, -1, 750, 493, 495, -1, 750, 495, 499, -1, 750, 499, 503, -1, 750, 503, 509, -1, 750, 509, 513, -1, 750, 513, 515, -1, 750, 515, 519, -1, 750, 519, 524, -1, 750, 524, 528, -1, 750, 528, 533, -1, 750, 533, 536, -1, 750, 536, 540, -1, 750, 540, 544, -1, 750, 544, 547, -1, 750, 547, 550, -1, 750, 550, 554, -1, 750, 554, 558, -1, 751, 752, 753, -1, 592, 751, 753, -1, 752, 754, 753, -1, 595, 592, 753, -1, 754, 595, 753, -1, 592, 731, 751, -1, 592, 416, 731, -1, 755, 756, 757, -1, 757, 756, 758, -1, 756, 759, 758, -1, 755, 757, 760, -1, 761, 755, 760, -1, 758, 759, 762, -1, 763, 764, 762, -1, 759, 763, 762, -1, 762, 764, 765, -1, 764, 766, 765, -1, 761, 760, 767, -1, 768, 761, 767, -1, 768, 767, 769, -1, 770, 768, 769, -1, 765, 766, 771, -1, 766, 772, 771, -1, 770, 769, 773, -1, 774, 770, 773, -1, 771, 772, 775, -1, 772, 776, 775, -1, 774, 773, 777, -1, 778, 774, 777, -1, 776, 779, 780, -1, 775, 776, 780, -1, 779, 781, 782, -1, 780, 779, 782, -1, 778, 777, 783, -1, 784, 778, 783, -1, 781, 785, 786, -1, 782, 781, 786, -1, 784, 783, 787, -1, 788, 784, 787, -1, 788, 787, 789, -1, 790, 788, 789, -1, 786, 785, 791, -1, 785, 792, 791, -1, 790, 789, 793, -1, 794, 790, 793, -1, 795, 794, 793, -1, 791, 792, 796, -1, 792, 797, 796, -1, 795, 793, 798, -1, 799, 795, 798, -1, 796, 797, 800, -1, 797, 801, 800, -1, 802, 799, 803, -1, 799, 798, 803, -1, 800, 801, 804, -1, 805, 806, 804, -1, 801, 805, 804, -1, 802, 803, 807, -1, 804, 806, 808, -1, 802, 807, 809, -1, 807, 810, 809, -1, 808, 806, 811, -1, 812, 808, 811, -1, 809, 810, 813, -1, 810, 814, 813, -1, 815, 812, 816, -1, 812, 811, 816, -1, 813, 814, 817, -1, 814, 818, 817, -1, 819, 815, 820, -1, 815, 816, 820, -1, 817, 818, 821, -1, 818, 822, 821, -1, 823, 819, 824, -1, 819, 820, 824, -1, 821, 822, 825, -1, 822, 826, 825, -1, 827, 823, 828, -1, 823, 824, 828, -1, 826, 829, 830, -1, 825, 826, 830, -1, 831, 827, 832, -1, 827, 828, 832, -1, 829, 833, 834, -1, 830, 829, 834, -1, 835, 831, 836, -1, 831, 832, 836, -1, 834, 833, 837, -1, 838, 835, 839, -1, 835, 836, 839, -1, 833, 840, 841, -1, 837, 833, 841, -1, 842, 838, 843, -1, 838, 839, 843, -1, 840, 844, 845, -1, 841, 840, 845, -1, 842, 843, 846, -1, 847, 842, 846, -1, 844, 848, 849, -1, 845, 844, 849, -1, 847, 846, 850, -1, 848, 851, 852, -1, 849, 848, 852, -1, 847, 850, 853, -1, 854, 847, 853, -1, 852, 851, 855, -1, 851, 856, 855, -1, 857, 854, 858, -1, 854, 853, 858, -1, 856, 859, 860, -1, 855, 856, 860, -1, 861, 857, 862, -1, 857, 858, 862, -1, 859, 861, 863, -1, 860, 859, 863, -1, 861, 862, 863, -1, 864, 865, 866, -1, 866, 865, 867, -1, 865, 868, 867, -1, 864, 866, 869, -1, 870, 864, 869, -1, 867, 868, 871, -1, 872, 873, 871, -1, 868, 872, 871, -1, 871, 873, 874, -1, 873, 875, 874, -1, 870, 869, 876, -1, 877, 870, 876, -1, 877, 876, 878, -1, 879, 877, 878, -1, 874, 875, 880, -1, 875, 881, 880, -1, 879, 878, 882, -1, 883, 879, 882, -1, 880, 881, 884, -1, 881, 885, 884, -1, 883, 882, 886, -1, 887, 883, 886, -1, 885, 888, 889, -1, 884, 885, 889, -1, 888, 890, 891, -1, 889, 888, 891, -1, 887, 886, 892, -1, 893, 887, 892, -1, 890, 894, 895, -1, 891, 890, 895, -1, 893, 892, 896, -1, 897, 893, 896, -1, 897, 896, 898, -1, 899, 897, 898, -1, 895, 894, 900, -1, 894, 901, 900, -1, 899, 898, 902, -1, 903, 899, 902, -1, 904, 903, 902, -1, 900, 901, 905, -1, 901, 906, 905, -1, 904, 902, 907, -1, 908, 904, 907, -1, 905, 906, 909, -1, 906, 910, 909, -1, 911, 908, 912, -1, 908, 907, 912, -1, 909, 910, 913, -1, 914, 915, 913, -1, 910, 914, 913, -1, 911, 912, 916, -1, 913, 915, 917, -1, 911, 916, 918, -1, 916, 919, 918, -1, 917, 915, 920, -1, 921, 917, 920, -1, 918, 919, 922, -1, 919, 923, 922, -1, 924, 921, 925, -1, 921, 920, 925, -1, 922, 923, 926, -1, 923, 927, 926, -1, 928, 924, 929, -1, 924, 925, 929, -1, 926, 927, 930, -1, 927, 931, 930, -1, 932, 928, 933, -1, 928, 929, 933, -1, 930, 931, 934, -1, 931, 935, 934, -1, 936, 932, 937, -1, 932, 933, 937, -1, 935, 938, 939, -1, 934, 935, 939, -1, 940, 936, 941, -1, 936, 937, 941, -1, 938, 942, 943, -1, 939, 938, 943, -1, 944, 940, 945, -1, 940, 941, 945, -1, 943, 942, 946, -1, 947, 944, 948, -1, 944, 945, 948, -1, 942, 949, 950, -1, 946, 942, 950, -1, 951, 947, 952, -1, 947, 948, 952, -1, 949, 953, 954, -1, 950, 949, 954, -1, 951, 952, 955, -1, 956, 951, 955, -1, 953, 957, 958, -1, 954, 953, 958, -1, 956, 955, 959, -1, 957, 960, 961, -1, 958, 957, 961, -1, 956, 959, 962, -1, 963, 956, 962, -1, 961, 960, 964, -1, 960, 965, 964, -1, 966, 963, 967, -1, 963, 962, 967, -1, 965, 968, 969, -1, 964, 965, 969, -1, 970, 966, 971, -1, 966, 967, 971, -1, 968, 970, 972, -1, 969, 968, 972, -1, 970, 971, 972, -1, 973, 974, 975, -1, 734, 737, 735, -1, 974, 737, 976, -1, 734, 975, 976, -1, 975, 974, 976, -1, 737, 734, 976, -1, 721, 719, 977, -1, 978, 721, 977, -1, 979, 980, 981, -1, 982, 723, 980, -1, 982, 724, 723, -1, 982, 983, 724, -1, 982, 979, 983, -1, 982, 980, 979, -1, 984, 742, 473, -1, 984, 473, 724, -1, 732, 448, 595, -1, 754, 732, 595, -1, 985, 986, 987, -1, 988, 985, 987, -1, 987, 989, 990, -1, 987, 986, 989, -1, 469, 991, 992, -1, 469, 993, 991, -1, 994, 992, 995, -1, 994, 995, 996, -1, 994, 996, 733, -1, 994, 733, 469, -1, 994, 469, 992, -1, 997, 998, 999, -1, 997, 733, 998, -1, 997, 734, 733, -1, 997, 975, 734, -1, 997, 999, 975, -1, 1000, 1001, 1002, -1, 1000, 1002, 1003, -1, 1000, 1004, 1001, -1, 1000, 1003, 1005, -1, 1000, 1005, 1004, -1, 1006, 1007, 1008, -1, 1009, 1010, 1001, -1, 1011, 1012, 1013, -1, 1001, 1004, 1014, -1, 1004, 1015, 1014, -1, 1015, 1016, 1014, -1, 1016, 1017, 1014, -1, 1017, 1013, 1014, -1, 1012, 1008, 1014, -1, 1007, 1009, 1014, -1, 1008, 1007, 1014, -1, 1009, 1001, 1014, -1, 1013, 1012, 1014, -1, 1005, 1018, 745, -1, 1005, 1003, 1018, -1, 741, 740, 742, -1, 741, 742, 1019, -1, 746, 1018, 1020, -1, 745, 1018, 746, -1, 747, 746, 1021, -1, 747, 1021, 1022, -1, 840, 833, 1023, -1, 844, 840, 1023, -1, 848, 844, 1023, -1, 851, 848, 1023, -1, 856, 851, 1023, -1, 859, 856, 1023, -1, 861, 859, 1023, -1, 857, 861, 1023, -1, 854, 857, 1023, -1, 847, 854, 1023, -1, 842, 847, 1023, -1, 838, 842, 1023, -1, 835, 838, 1023, -1, 831, 835, 1023, -1, 827, 831, 1023, -1, 823, 827, 1023, -1, 819, 823, 1023, -1, 815, 819, 1023, -1, 812, 815, 1023, -1, 808, 812, 1023, -1, 804, 808, 1023, -1, 800, 804, 1023, -1, 796, 800, 1023, -1, 791, 796, 1023, -1, 786, 791, 1023, -1, 782, 786, 1023, -1, 780, 782, 1023, -1, 775, 780, 1023, -1, 771, 775, 1023, -1, 765, 771, 1023, -1, 762, 765, 1023, -1, 758, 762, 1023, -1, 757, 758, 1023, -1, 760, 757, 1023, -1, 767, 760, 1023, -1, 769, 767, 1023, -1, 773, 769, 1023, -1, 777, 773, 1023, -1, 783, 777, 1023, -1, 787, 783, 1023, -1, 789, 787, 1023, -1, 793, 789, 1023, -1, 798, 793, 1023, -1, 803, 798, 1023, -1, 807, 803, 1023, -1, 810, 807, 1023, -1, 814, 810, 1023, -1, 818, 814, 1023, -1, 822, 818, 1023, -1, 826, 822, 1023, -1, 829, 826, 1023, -1, 833, 829, 1023, -1, 949, 942, 1024, -1, 953, 949, 1024, -1, 957, 953, 1024, -1, 960, 957, 1024, -1, 965, 960, 1024, -1, 968, 965, 1024, -1, 970, 968, 1024, -1, 966, 970, 1024, -1, 963, 966, 1024, -1, 956, 963, 1024, -1, 951, 956, 1024, -1, 947, 951, 1024, -1, 944, 947, 1024, -1, 940, 944, 1024, -1, 936, 940, 1024, -1, 932, 936, 1024, -1, 928, 932, 1024, -1, 924, 928, 1024, -1, 921, 924, 1024, -1, 917, 921, 1024, -1, 913, 917, 1024, -1, 909, 913, 1024, -1, 905, 909, 1024, -1, 900, 905, 1024, -1, 895, 900, 1024, -1, 891, 895, 1024, -1, 889, 891, 1024, -1, 884, 889, 1024, -1, 880, 884, 1024, -1, 874, 880, 1024, -1, 871, 874, 1024, -1, 867, 871, 1024, -1, 866, 867, 1024, -1, 869, 866, 1024, -1, 876, 869, 1024, -1, 878, 876, 1024, -1, 882, 878, 1024, -1, 886, 882, 1024, -1, 892, 886, 1024, -1, 896, 892, 1024, -1, 898, 896, 1024, -1, 902, 898, 1024, -1, 907, 902, 1024, -1, 912, 907, 1024, -1, 916, 912, 1024, -1, 919, 916, 1024, -1, 923, 919, 1024, -1, 927, 923, 1024, -1, 931, 927, 1024, -1, 935, 931, 1024, -1, 938, 935, 1024, -1, 942, 938, 1024, -1, 1025, 973, 975, -1, 1025, 981, 973, -1, 1025, 979, 981, -1, 1025, 975, 979, -1, 1026, 1027, 1028, -1, 984, 724, 1027, -1, 1029, 724, 983, -1, 1029, 983, 1028, -1, 1029, 1027, 724, -1, 1029, 1028, 1027, -1, 1030, 1031, 1032, -1, 1033, 983, 979, -1, 1033, 1028, 983, -1, 1033, 1031, 1028, -1, 1033, 979, 1032, -1, 1033, 1032, 1031, -1, 1034, 1035, 984, -1, 1035, 742, 984, -1, 1036, 1037, 1038, -1, 1039, 1040, 1041, -1, 1042, 1038, 1043, -1, 1042, 1043, 1040, -1, 1042, 1039, 1038, -1, 1042, 1040, 1039, -1, 747, 1022, 1036, -1, 1044, 1039, 1045, -1, 1046, 1047, 1039, -1, 1046, 1048, 1047, -1, 1046, 1049, 1048, -1, 1046, 1044, 1049, -1, 1046, 1039, 1044, -1, 999, 1050, 975, -1, 979, 1051, 1032, -1, 1050, 1051, 1052, -1, 979, 975, 1052, -1, 975, 1050, 1052, -1, 1051, 979, 1052, -1, 1053, 1054, 1055, -1, 1001, 1010, 1053, -1, 1056, 1057, 1058, -1, 1056, 1059, 1057, -1, 1056, 1055, 1059, -1, 1056, 1002, 1001, -1, 1056, 1058, 1002, -1, 1056, 1001, 1053, -1, 1056, 1053, 1055, -1, 1060, 1002, 1058, -1, 1060, 1003, 1002, -1, 1060, 1018, 1003, -1, 1060, 1061, 1018, -1, 1060, 1058, 1061, -1, 1019, 742, 1062, -1, 742, 1035, 1062, -1, 1063, 1019, 1062, -1, 1035, 1063, 1062, -1, 1017, 1064, 1013, -1, 1064, 1065, 1013, -1, 1013, 1065, 1011, -1, 1065, 1064, 1066, -1, 1064, 1067, 1066, -1, 1067, 1068, 1066, -1, 1068, 1011, 1066, -1, 1011, 1065, 1066, -1, 1012, 1069, 1070, -1, 1071, 1072, 1069, -1, 1071, 1068, 1072, -1, 1071, 1011, 1068, -1, 1071, 1012, 1011, -1, 1071, 1069, 1012, -1, 1070, 1008, 1012, -1, 1069, 1073, 1074, -1, 1075, 1070, 1069, -1, 1075, 1008, 1070, -1, 1075, 1006, 1008, -1, 1075, 1074, 1006, -1, 1075, 1069, 1074, -1, 1076, 1077, 1006, -1, 1076, 1006, 1074, -1, 1077, 1078, 1007, -1, 1006, 1077, 1007, -1, 1054, 1053, 1079, -1, 1053, 1080, 1079, -1, 1080, 1007, 1079, -1, 1078, 1054, 1079, -1, 1007, 1078, 1079, -1, 1007, 1080, 1009, -1, 1053, 1010, 1080, -1, 1080, 1010, 1009, -1, 1020, 1061, 1081, -1, 1018, 1061, 1020, -1, 1036, 1082, 1037, -1, 1082, 746, 1083, -1, 746, 1020, 1083, -1, 1020, 1084, 1083, -1, 1084, 1037, 1083, -1, 1037, 1082, 1083, -1, 1082, 1021, 746, -1, 1021, 1082, 1036, -1, 1022, 1021, 1036, -1, 984, 1027, 1026, -1, 1085, 984, 1026, -1, 1086, 1087, 1088, -1, 1089, 1090, 1086, -1, 1091, 1086, 1088, -1, 1091, 1089, 1086, -1, 1092, 1091, 1093, -1, 1093, 1091, 1088, -1, 1086, 1094, 1087, -1, 1086, 1090, 1094, -1, 1088, 1087, 1095, -1, 1096, 1067, 1064, -1, 1096, 1064, 1097, -1, 1098, 1034, 984, -1, 1098, 984, 1085, -1, 1098, 1099, 1034, -1, 1098, 1085, 1099, -1, 1084, 1100, 1037, -1, 1037, 1100, 1038, -1, 1101, 1102, 1103, -1, 1102, 1100, 1103, -1, 1084, 1101, 1103, -1, 1100, 1084, 1103, -1, 1100, 1104, 1038, -1, 1038, 1104, 1043, -1, 1043, 1105, 1040, -1, 1043, 1104, 1105, -1, 1040, 1105, 1041, -1, 1105, 1106, 1041, -1, 1045, 1039, 1041, -1, 1044, 1045, 1107, -1, 1107, 1108, 1044, -1, 1109, 1110, 1111, -1, 1109, 1107, 1110, -1, 1109, 1111, 1108, -1, 1109, 1108, 1107, -1, 1112, 1113, 1114, -1, 1112, 1114, 1115, -1, 1116, 1112, 1117, -1, 1111, 1116, 1117, -1, 1108, 1111, 1117, -1, 1115, 1108, 1117, -1, 1112, 1115, 1117, -1, 1095, 1115, 1118, -1, 1095, 1118, 1088, -1, 1061, 1057, 1119, -1, 1058, 1057, 1061, -1, 1120, 1121, 1122, -1, 1120, 1123, 1121, -1, 1124, 1123, 1120, -1, 1120, 1122, 1125, -1, 1122, 1057, 1126, -1, 1057, 1059, 1126, -1, 1059, 1125, 1126, -1, 1125, 1122, 1126, -1, 1059, 1055, 1127, -1, 1055, 1128, 1127, -1, 1128, 1129, 1127, -1, 1129, 1130, 1127, -1, 1130, 1059, 1127, -1, 1131, 1055, 1054, -1, 1131, 1128, 1055, -1, 1131, 1078, 1128, -1, 1131, 1054, 1078, -1, 1132, 1068, 1067, -1, 1132, 1067, 1096, -1, 1073, 1069, 1076, -1, 1133, 1077, 1076, -1, 1133, 1134, 1077, -1, 1133, 1135, 1134, -1, 1133, 1072, 1135, -1, 1133, 1069, 1072, -1, 1133, 1076, 1069, -1, 1136, 1137, 1138, -1, 1139, 1140, 1136, -1, 1139, 1068, 1140, -1, 1139, 1072, 1068, -1, 1139, 1138, 1072, -1, 1139, 1136, 1138, -1, 1076, 1074, 1073, -1, 1078, 1077, 1128, -1, 1129, 1128, 1134, -1, 1128, 1077, 1134, -1, 1081, 1119, 1141, -1, 1061, 1119, 1081, -1, 1020, 1081, 1084, -1, 1081, 1142, 1084, -1, 1143, 1026, 1144, -1, 1085, 1026, 1143, -1, 1145, 1085, 1143, -1, 1146, 1092, 1093, -1, 1146, 1147, 1092, -1, 1093, 1088, 1148, -1, 1149, 1150, 1146, -1, 1151, 1150, 1149, -1, 1152, 1146, 1093, -1, 1152, 1148, 1149, -1, 1152, 1093, 1148, -1, 1152, 1149, 1146, -1, 1153, 1154, 1155, -1, 1156, 1096, 1097, -1, 1156, 1132, 1096, -1, 1156, 1153, 1132, -1, 1156, 1097, 1154, -1, 1156, 1154, 1153, -1, 1155, 1097, 1157, -1, 1155, 1154, 1097, -1, 1157, 1158, 1159, -1, 1102, 1101, 1106, -1, 1160, 1161, 1162, -1, 1101, 1160, 1162, -1, 1161, 1106, 1162, -1, 1106, 1101, 1162, -1, 1106, 1105, 1102, -1, 1105, 1104, 1163, -1, 1104, 1100, 1163, -1, 1100, 1102, 1163, -1, 1102, 1105, 1163, -1, 1164, 1160, 1165, -1, 1160, 1101, 1165, -1, 1101, 1084, 1165, -1, 1084, 1142, 1165, -1, 1142, 1166, 1165, -1, 1166, 1164, 1165, -1, 1041, 1106, 1045, -1, 1106, 1167, 1045, -1, 1045, 1167, 1107, -1, 1168, 1110, 1169, -1, 1110, 1107, 1169, -1, 1167, 1168, 1169, -1, 1107, 1167, 1169, -1, 1170, 1171, 1116, -1, 1170, 1110, 1171, -1, 1170, 1111, 1110, -1, 1170, 1116, 1111, -1, 1171, 1161, 1172, -1, 1173, 1161, 1171, -1, 1160, 1172, 1161, -1, 1164, 1174, 1172, -1, 1164, 1172, 1160, -1, 1175, 1116, 1171, -1, 1175, 1171, 1172, -1, 1112, 1116, 1175, -1, 1113, 1176, 1177, -1, 1175, 1113, 1112, -1, 1177, 1176, 1178, -1, 1176, 1179, 1180, -1, 1179, 1176, 1181, -1, 1182, 1181, 1183, -1, 1181, 1182, 1179, -1, 1175, 1180, 1184, -1, 1180, 1184, 1185, -1, 1178, 1180, 1185, -1, 1176, 1180, 1178, -1, 1175, 1176, 1180, -1, 1177, 1186, 1187, -1, 1186, 1114, 1187, -1, 1114, 1113, 1187, -1, 1113, 1177, 1187, -1, 1115, 1114, 1186, -1, 1115, 1186, 1188, -1, 1118, 1188, 1115, -1, 1088, 1118, 1148, -1, 1118, 1188, 1189, -1, 1188, 1186, 1189, -1, 1186, 1190, 1189, -1, 1190, 1148, 1189, -1, 1148, 1118, 1189, -1, 1191, 1192, 1119, -1, 1192, 1141, 1119, -1, 1191, 1119, 1057, -1, 1193, 1123, 1124, -1, 1193, 1124, 1194, -1, 1123, 1191, 1057, -1, 1057, 1122, 1195, -1, 1122, 1121, 1195, -1, 1121, 1123, 1195, -1, 1123, 1057, 1195, -1, 1196, 1197, 1198, -1, 1197, 1125, 1198, -1, 1125, 1059, 1198, -1, 1059, 1196, 1198, -1, 1199, 1200, 1201, -1, 1202, 1203, 1201, -1, 1201, 1203, 1176, -1, 1199, 1201, 1175, -1, 1201, 1176, 1175, -1, 1125, 1199, 1204, -1, 1120, 1125, 1204, -1, 1172, 1120, 1204, -1, 1175, 1172, 1204, -1, 1199, 1175, 1204, -1, 1194, 1124, 1120, -1, 1120, 1172, 1205, -1, 1174, 1194, 1205, -1, 1172, 1174, 1205, -1, 1194, 1120, 1205, -1, 1134, 1130, 1129, -1, 1206, 1207, 1130, -1, 1206, 1135, 1207, -1, 1206, 1134, 1135, -1, 1206, 1130, 1134, -1, 1208, 1209, 1210, -1, 1208, 1210, 1211, -1, 1211, 1210, 1212, -1, 1208, 1213, 1214, -1, 1213, 1215, 1214, -1, 1215, 1216, 1214, -1, 1217, 1209, 1214, -1, 1216, 1217, 1214, -1, 1209, 1208, 1214, -1, 1218, 1140, 1068, -1, 1218, 1068, 1132, -1, 1219, 1220, 1221, -1, 1135, 1219, 1207, -1, 1135, 1220, 1219, -1, 1222, 1223, 1224, -1, 1222, 1072, 1223, -1, 1222, 1135, 1072, -1, 1222, 1224, 1220, -1, 1222, 1220, 1135, -1, 1225, 1226, 1227, -1, 1228, 1226, 1225, -1, 1229, 1228, 1225, -1, 1230, 1229, 1225, -1, 1231, 1225, 1232, -1, 1231, 1232, 1233, -1, 1231, 1230, 1225, -1, 1234, 1140, 1218, -1, 1218, 1235, 1236, -1, 1234, 1218, 1236, -1, 1237, 1136, 1238, -1, 1234, 1237, 1238, -1, 1136, 1140, 1238, -1, 1140, 1234, 1238, -1, 1239, 1137, 1224, -1, 1239, 1138, 1137, -1, 1239, 1072, 1138, -1, 1239, 1223, 1072, -1, 1239, 1224, 1223, -1, 1192, 1240, 1241, -1, 1241, 1142, 1242, -1, 1142, 1081, 1242, -1, 1081, 1141, 1242, -1, 1141, 1192, 1242, -1, 1192, 1241, 1242, -1, 1147, 1159, 1092, -1, 1153, 1155, 1243, -1, 1146, 1150, 1244, -1, 1146, 1244, 1147, -1, 1245, 1150, 1243, -1, 1245, 1155, 1244, -1, 1245, 1243, 1155, -1, 1245, 1244, 1150, -1, 1246, 1150, 1151, -1, 1149, 1247, 1151, -1, 1149, 1248, 1249, -1, 1248, 1250, 1249, -1, 1250, 1247, 1249, -1, 1247, 1149, 1249, -1, 1148, 1248, 1149, -1, 1148, 1190, 1248, -1, 1218, 1132, 1235, -1, 1251, 1252, 1235, -1, 1251, 1153, 1252, -1, 1251, 1132, 1153, -1, 1251, 1235, 1132, -1, 1147, 1157, 1159, -1, 1155, 1157, 1147, -1, 1244, 1155, 1147, -1, 1106, 1168, 1167, -1, 1161, 1173, 1253, -1, 1106, 1161, 1253, -1, 1173, 1168, 1253, -1, 1168, 1106, 1253, -1, 1241, 1254, 1123, -1, 1241, 1123, 1193, -1, 1241, 1193, 1142, -1, 1142, 1193, 1166, -1, 1166, 1193, 1194, -1, 1164, 1166, 1174, -1, 1194, 1174, 1166, -1, 1173, 1171, 1255, -1, 1171, 1110, 1255, -1, 1110, 1168, 1255, -1, 1168, 1173, 1255, -1, 1185, 1184, 1256, -1, 1186, 1178, 1190, -1, 1190, 1178, 1185, -1, 1248, 1257, 1250, -1, 1185, 1256, 1258, -1, 1256, 1257, 1258, -1, 1248, 1190, 1258, -1, 1190, 1185, 1258, -1, 1257, 1248, 1258, -1, 1178, 1186, 1259, -1, 1186, 1177, 1259, -1, 1177, 1178, 1259, -1, 1181, 1203, 1260, -1, 1176, 1203, 1181, -1, 1261, 1260, 1262, -1, 1183, 1181, 1261, -1, 1181, 1260, 1261, -1, 1182, 1183, 1261, -1, 1263, 1179, 1182, -1, 1264, 1263, 1182, -1, 1265, 1266, 1264, -1, 1265, 1262, 1266, -1, 1265, 1261, 1262, -1, 1265, 1182, 1261, -1, 1265, 1264, 1182, -1, 1180, 1179, 1263, -1, 1267, 1268, 1269, -1, 1267, 1269, 1270, -1, 1263, 1264, 1180, -1, 1180, 1264, 1184, -1, 1271, 1256, 1184, -1, 1271, 1257, 1256, -1, 1271, 1269, 1257, -1, 1271, 1264, 1270, -1, 1271, 1270, 1269, -1, 1271, 1184, 1264, -1, 1123, 1254, 1191, -1, 1272, 1254, 1240, -1, 1272, 1240, 1192, -1, 1272, 1192, 1191, -1, 1272, 1191, 1254, -1, 1262, 1202, 1196, -1, 1202, 1197, 1196, -1, 1262, 1196, 1273, -1, 1274, 1275, 1125, -1, 1274, 1125, 1276, -1, 1276, 1125, 1197, -1, 1125, 1275, 1277, -1, 1275, 1278, 1277, -1, 1278, 1199, 1277, -1, 1199, 1125, 1277, -1, 1279, 1200, 1199, -1, 1279, 1276, 1200, -1, 1279, 1274, 1276, -1, 1279, 1278, 1274, -1, 1279, 1199, 1278, -1, 1200, 1276, 1201, -1, 1276, 1280, 1201, -1, 1201, 1280, 1202, -1, 1260, 1203, 1262, -1, 1203, 1202, 1262, -1, 1130, 1207, 1281, -1, 1207, 1219, 1281, -1, 1219, 1282, 1281, -1, 1282, 1130, 1281, -1, 1283, 1284, 1285, -1, 1283, 1286, 1284, -1, 1287, 1286, 1283, -1, 1287, 1283, 1288, -1, 1288, 1283, 1289, -1, 1287, 1290, 1286, -1, 1291, 1213, 1208, -1, 1292, 1291, 1293, -1, 1291, 1208, 1293, -1, 1286, 1290, 1284, -1, 1290, 1282, 1284, -1, 1294, 1130, 1295, -1, 1295, 1130, 1282, -1, 1221, 1296, 1219, -1, 1268, 1250, 1269, -1, 1284, 1297, 1285, -1, 1268, 1297, 1298, -1, 1284, 1282, 1298, -1, 1282, 1219, 1298, -1, 1296, 1299, 1298, -1, 1299, 1250, 1298, -1, 1219, 1296, 1298, -1, 1250, 1268, 1298, -1, 1297, 1284, 1298, -1, 1300, 1301, 1302, -1, 1303, 1304, 1305, -1, 1306, 1231, 1233, -1, 1306, 1300, 1231, -1, 1306, 1304, 1301, -1, 1306, 1233, 1305, -1, 1306, 1305, 1304, -1, 1306, 1301, 1300, -1, 1307, 1232, 1225, -1, 1307, 1308, 1232, -1, 1309, 1307, 1225, -1, 1309, 1225, 1227, -1, 1227, 1310, 1311, -1, 1310, 1312, 1311, -1, 1312, 1313, 1311, -1, 1313, 1309, 1311, -1, 1309, 1227, 1311, -1, 1314, 1315, 1316, -1, 1227, 1226, 1317, -1, 1304, 1318, 1319, -1, 1303, 1318, 1304, -1, 1320, 1310, 1227, -1, 1320, 1321, 1310, -1, 1320, 1319, 1321, -1, 1320, 1316, 1304, -1, 1320, 1317, 1314, -1, 1320, 1314, 1316, -1, 1320, 1227, 1317, -1, 1320, 1304, 1319, -1, 1308, 1233, 1232, -1, 1307, 1233, 1308, -1, 1322, 1305, 1233, -1, 1322, 1323, 1305, -1, 1322, 1324, 1323, -1, 1322, 1307, 1324, -1, 1322, 1233, 1307, -1, 1325, 1326, 1327, -1, 1325, 1234, 1326, -1, 1325, 1237, 1234, -1, 1325, 1328, 1237, -1, 1325, 1327, 1328, -1, 1329, 1330, 1331, -1, 1332, 1329, 1331, -1, 1236, 1235, 1333, -1, 1235, 1334, 1333, -1, 1335, 1333, 1336, -1, 1337, 1335, 1336, -1, 1338, 1337, 1336, -1, 1334, 1338, 1336, -1, 1333, 1334, 1336, -1, 1254, 1241, 1240, -1, 1150, 1153, 1243, -1, 1246, 1153, 1150, -1, 1252, 1153, 1246, -1, 1151, 1339, 1246, -1, 1339, 1151, 1247, -1, 1247, 1340, 1341, -1, 1340, 1342, 1341, -1, 1342, 1339, 1341, -1, 1339, 1247, 1341, -1, 1247, 1250, 1340, -1, 1250, 1299, 1338, -1, 1338, 1334, 1343, -1, 1334, 1342, 1343, -1, 1342, 1340, 1343, -1, 1340, 1250, 1343, -1, 1250, 1338, 1343, -1, 1342, 1334, 1235, -1, 1252, 1342, 1235, -1, 1269, 1250, 1257, -1, 1344, 1288, 1289, -1, 1344, 1289, 1270, -1, 1344, 1266, 1288, -1, 1344, 1270, 1264, -1, 1344, 1264, 1266, -1, 1267, 1297, 1268, -1, 1345, 1297, 1267, -1, 1289, 1283, 1346, -1, 1283, 1345, 1346, -1, 1345, 1267, 1346, -1, 1267, 1270, 1346, -1, 1270, 1289, 1346, -1, 1280, 1276, 1197, -1, 1280, 1197, 1202, -1, 1278, 1275, 1274, -1, 1283, 1285, 1345, -1, 1345, 1285, 1297, -1, 1347, 1335, 1348, -1, 1296, 1349, 1350, -1, 1349, 1347, 1350, -1, 1348, 1299, 1350, -1, 1299, 1296, 1350, -1, 1347, 1348, 1350, -1, 1351, 1352, 1353, -1, 1352, 1347, 1353, -1, 1305, 1323, 1303, -1, 1323, 1354, 1355, -1, 1354, 1356, 1355, -1, 1356, 1318, 1355, -1, 1318, 1303, 1355, -1, 1303, 1323, 1355, -1, 1357, 1358, 1307, -1, 1357, 1307, 1309, -1, 1357, 1359, 1358, -1, 1357, 1309, 1359, -1, 1360, 1361, 1362, -1, 1363, 1360, 1312, -1, 1360, 1362, 1312, -1, 1310, 1321, 1364, -1, 1321, 1362, 1364, -1, 1312, 1310, 1364, -1, 1361, 1363, 1364, -1, 1363, 1312, 1364, -1, 1362, 1361, 1364, -1, 1365, 1366, 1367, -1, 1368, 1365, 1367, -1, 1369, 1368, 1367, -1, 1370, 1369, 1367, -1, 1371, 1372, 1373, -1, 1371, 1373, 1370, -1, 1371, 1367, 1372, -1, 1371, 1370, 1367, -1, 1374, 1375, 1373, -1, 1375, 1370, 1373, -1, 1321, 1319, 1376, -1, 1321, 1376, 1362, -1, 1377, 1378, 1356, -1, 1378, 1376, 1356, -1, 1377, 1356, 1379, -1, 1376, 1319, 1318, -1, 1376, 1318, 1356, -1, 1335, 1347, 1327, -1, 1347, 1380, 1327, -1, 1381, 1327, 1382, -1, 1381, 1328, 1327, -1, 1381, 1237, 1328, -1, 1381, 1383, 1237, -1, 1381, 1382, 1383, -1, 1384, 1385, 1386, -1, 1386, 1385, 1387, -1, 1387, 1385, 1388, -1, 1388, 1389, 1390, -1, 1389, 1391, 1390, -1, 1391, 1387, 1390, -1, 1387, 1388, 1390, -1, 1392, 1393, 1391, -1, 1394, 1395, 1392, -1, 1394, 1389, 1395, -1, 1394, 1391, 1389, -1, 1394, 1392, 1391, -1, 1333, 1327, 1326, -1, 1333, 1335, 1327, -1, 1348, 1335, 1396, -1, 1299, 1348, 1396, -1, 1335, 1337, 1396, -1, 1338, 1299, 1396, -1, 1337, 1338, 1396, -1, 1339, 1342, 1252, -1, 1246, 1339, 1252, -1, 1385, 1384, 1397, -1, 1385, 1397, 1398, -1, 1398, 1397, 1399, -1, 1400, 1392, 1395, -1, 1400, 1401, 1392, -1, 1400, 1402, 1401, -1, 1400, 1403, 1402, -1, 1400, 1395, 1403, -1, 1404, 1403, 1374, -1, 1404, 1374, 1405, -1, 1406, 1407, 1408, -1, 1409, 1406, 1408, -1, 1407, 1406, 1409, -1, 1410, 1411, 1412, -1, 1410, 1413, 1411, -1, 1414, 1410, 1412, -1, 1372, 1415, 1373, -1, 1372, 1416, 1415, -1, 1372, 1417, 1416, -1, 1418, 1372, 1410, -1, 1418, 1414, 1417, -1, 1418, 1410, 1414, -1, 1418, 1417, 1372, -1, 1419, 1420, 1421, -1, 1422, 1423, 1424, -1, 1422, 1367, 1425, -1, 1372, 1367, 1422, -1, 1421, 1410, 1419, -1, 1426, 1424, 1427, -1, 1426, 1410, 1372, -1, 1426, 1427, 1419, -1, 1426, 1419, 1410, -1, 1426, 1422, 1424, -1, 1426, 1372, 1422, -1, 1366, 1428, 1367, -1, 1428, 1425, 1367, -1, 1429, 1422, 1425, -1, 1429, 1365, 1422, -1, 1429, 1366, 1365, -1, 1429, 1428, 1366, -1, 1429, 1425, 1428, -1, 1373, 1415, 1430, -1, 1430, 1405, 1431, -1, 1405, 1374, 1431, -1, 1374, 1373, 1431, -1, 1373, 1430, 1431, -1, 1432, 1424, 1423, -1, 1432, 1423, 1399, -1, 1432, 1399, 1397, -1, 1391, 1433, 1434, -1, 1433, 1384, 1434, -1, 1384, 1386, 1434, -1, 1386, 1387, 1434, -1, 1387, 1391, 1434, -1, 1391, 1435, 1436, -1, 1437, 1393, 1438, -1, 1437, 1391, 1393, -1, 1437, 1439, 1435, -1, 1437, 1438, 1439, -1, 1437, 1435, 1391, -1, 1440, 1438, 1393, -1, 1393, 1392, 1441, -1, 1392, 1401, 1441, -1, 1401, 1442, 1441, -1, 1442, 1443, 1441, -1, 1443, 1444, 1441, -1, 1444, 1445, 1441, -1, 1445, 1446, 1441, -1, 1446, 1447, 1441, -1, 1447, 1440, 1441, -1, 1440, 1393, 1441, -1, 1448, 1449, 1384, -1, 1450, 1448, 1451, -1, 1452, 1450, 1451, -1, 1452, 1432, 1453, -1, 1432, 1397, 1453, -1, 1397, 1384, 1453, -1, 1449, 1450, 1453, -1, 1450, 1452, 1453, -1, 1384, 1449, 1453, -1, 1442, 1401, 1454, -1, 1401, 1402, 1455, -1, 1402, 1456, 1455, -1, 1456, 1457, 1455, -1, 1457, 1454, 1455, -1, 1454, 1401, 1455, -1, 1402, 1403, 1404, -1, 1458, 1402, 1404, -1, 1459, 1460, 1461, -1, 1462, 1463, 1464, -1, 1463, 1465, 1464, -1, 1466, 1459, 1464, -1, 1466, 1465, 1460, -1, 1466, 1464, 1465, -1, 1466, 1460, 1459, -1, 1421, 1467, 1468, -1, 1421, 1468, 1469, -1, 1421, 1469, 1470, -1, 1410, 1421, 1471, -1, 1413, 1410, 1471, -1, 1470, 1413, 1471, -1, 1421, 1470, 1471, -1, 1430, 1415, 1472, -1, 1415, 1416, 1472, -1, 1414, 1412, 1473, -1, 1474, 1414, 1473, -1, 1416, 1417, 1475, -1, 1472, 1416, 1475, -1, 1476, 1474, 1475, -1, 1476, 1417, 1414, -1, 1476, 1414, 1474, -1, 1476, 1475, 1417, -1, 1412, 1411, 1477, -1, 1473, 1412, 1477, -1, 1411, 1413, 1477, -1, 1478, 1479, 1480, -1, 1413, 1478, 1480, -1, 1479, 1477, 1480, -1, 1477, 1413, 1480, -1, 1424, 1432, 1427, -1, 1481, 1482, 1427, -1, 1481, 1452, 1482, -1, 1481, 1432, 1452, -1, 1481, 1427, 1432, -1, 1483, 1467, 1421, -1, 1483, 1421, 1420, -1, 1484, 1420, 1419, -1, 1484, 1419, 1468, -1, 1484, 1483, 1420, -1, 1484, 1468, 1467, -1, 1484, 1467, 1483, -1, 1419, 1427, 1482, -1, 1419, 1482, 1468, -1, 1468, 1485, 1486, -1, 1486, 1485, 1487, -1, 1487, 1488, 1489, -1, 1486, 1487, 1489, -1, 1486, 1489, 1490, -1, 1490, 1489, 1491, -1, 1468, 1482, 1485, -1, 1485, 1492, 1487, -1, 1448, 1384, 1433, -1, 1448, 1433, 1436, -1, 1436, 1433, 1391, -1, 1493, 1494, 1495, -1, 1495, 1494, 1496, -1, 1495, 1436, 1497, -1, 1436, 1435, 1497, -1, 1435, 1498, 1497, -1, 1498, 1493, 1497, -1, 1493, 1495, 1497, -1, 1447, 1439, 1440, -1, 1499, 1500, 1498, -1, 1501, 1447, 1499, -1, 1501, 1435, 1439, -1, 1501, 1498, 1435, -1, 1501, 1439, 1447, -1, 1501, 1499, 1498, -1, 1440, 1439, 1438, -1, 1443, 1442, 1454, -1, 1502, 1443, 1457, -1, 1443, 1454, 1457, -1, 1502, 1443, 1444, -1, 1444, 1502, 1503, -1, 1445, 1444, 1504, -1, 1444, 1503, 1504, -1, 1446, 1445, 1504, -1, 1503, 1505, 1506, -1, 1505, 1507, 1506, -1, 1507, 1446, 1506, -1, 1504, 1503, 1506, -1, 1446, 1504, 1506, -1, 1499, 1447, 1446, -1, 1507, 1499, 1446, -1, 1448, 1436, 1508, -1, 1436, 1509, 1508, -1, 1509, 1451, 1508, -1, 1451, 1448, 1508, -1, 1510, 1485, 1482, -1, 1510, 1509, 1485, -1, 1510, 1451, 1509, -1, 1510, 1452, 1451, -1, 1510, 1482, 1452, -1, 1449, 1511, 1450, -1, 1511, 1448, 1450, -1, 1511, 1449, 1448, -1, 1512, 1402, 1458, -1, 1512, 1456, 1402, -1, 1461, 1513, 1459, -1, 1461, 1514, 1513, -1, 1460, 1465, 1515, -1, 1460, 1515, 1516, -1, 1461, 1516, 1517, -1, 1461, 1460, 1516, -1, 1469, 1468, 1518, -1, 1469, 1518, 1519, -1, 1518, 1468, 1520, -1, 1468, 1486, 1521, -1, 1486, 1522, 1521, -1, 1522, 1523, 1521, -1, 1523, 1520, 1521, -1, 1520, 1468, 1521, -1, 1519, 1524, 1470, -1, 1469, 1519, 1470, -1, 1470, 1524, 1523, -1, 1525, 1526, 1522, -1, 1527, 1525, 1528, -1, 1525, 1522, 1528, -1, 1526, 1523, 1522, -1, 1528, 1470, 1529, -1, 1526, 1527, 1529, -1, 1470, 1523, 1529, -1, 1523, 1526, 1529, -1, 1527, 1528, 1529, -1, 1478, 1528, 1530, -1, 1470, 1528, 1531, -1, 1478, 1413, 1531, -1, 1413, 1470, 1531, -1, 1528, 1478, 1531, -1, 1517, 1516, 1532, -1, 1533, 1517, 1532, -1, 1534, 1473, 1477, -1, 1534, 1532, 1473, -1, 1534, 1533, 1532, -1, 1535, 1536, 1479, -1, 1478, 1535, 1479, -1, 1537, 1538, 1539, -1, 1536, 1537, 1539, -1, 1538, 1479, 1539, -1, 1479, 1536, 1539, -1, 1540, 1541, 1542, -1, 1543, 1477, 1541, -1, 1543, 1534, 1477, -1, 1543, 1533, 1534, -1, 1543, 1540, 1533, -1, 1543, 1541, 1540, -1, 1491, 1489, 1544, -1, 1489, 1545, 1544, -1, 1545, 1546, 1544, -1, 1546, 1491, 1544, -1, 1547, 1548, 1549, -1, 1490, 1491, 1550, -1, 1551, 1550, 1547, -1, 1551, 1552, 1490, -1, 1551, 1537, 1552, -1, 1551, 1553, 1537, -1, 1551, 1554, 1553, -1, 1551, 1549, 1554, -1, 1551, 1490, 1550, -1, 1551, 1547, 1549, -1, 1522, 1486, 1490, -1, 1528, 1522, 1490, -1, 1490, 1552, 1555, -1, 1552, 1530, 1555, -1, 1530, 1528, 1555, -1, 1528, 1490, 1555, -1, 1556, 1509, 1436, -1, 1556, 1485, 1509, -1, 1556, 1492, 1485, -1, 1556, 1436, 1492, -1, 1436, 1495, 1557, -1, 1492, 1436, 1557, -1, 1495, 1487, 1557, -1, 1487, 1492, 1557, -1, 1558, 1495, 1496, -1, 1487, 1558, 1488, -1, 1487, 1495, 1558, -1, 1545, 1489, 1488, -1, 1545, 1488, 1558, -1, 1559, 1494, 1493, -1, 1559, 1560, 1494, -1, 1559, 1561, 1560, -1, 1559, 1493, 1561, -1, 1546, 1545, 1558, -1, 1560, 1496, 1494, -1, 1562, 1563, 1546, -1, 1562, 1560, 1563, -1, 1562, 1558, 1496, -1, 1562, 1546, 1558, -1, 1562, 1496, 1560, -1, 1493, 1498, 1561, -1, 1498, 1564, 1561, -1, 1498, 1500, 1565, -1, 1500, 1564, 1565, -1, 1564, 1498, 1565, -1, 1564, 1549, 1566, -1, 1554, 1549, 1564, -1, 1567, 1505, 1554, -1, 1567, 1507, 1505, -1, 1567, 1499, 1507, -1, 1567, 1500, 1499, -1, 1567, 1564, 1500, -1, 1567, 1554, 1564, -1, 1568, 1514, 1569, -1, 1570, 1553, 1505, -1, 1571, 1503, 1572, -1, 1571, 1572, 1570, -1, 1571, 1505, 1503, -1, 1571, 1570, 1505, -1, 1533, 1569, 1514, -1, 1533, 1514, 1461, -1, 1533, 1461, 1517, -1, 1519, 1518, 1573, -1, 1518, 1520, 1573, -1, 1520, 1523, 1573, -1, 1523, 1519, 1573, -1, 1523, 1524, 1574, -1, 1524, 1519, 1574, -1, 1519, 1523, 1574, -1, 1526, 1525, 1575, -1, 1525, 1527, 1575, -1, 1527, 1526, 1575, -1, 1576, 1530, 1535, -1, 1576, 1478, 1530, -1, 1576, 1535, 1478, -1, 1538, 1537, 1553, -1, 1538, 1553, 1570, -1, 1577, 1537, 1536, -1, 1577, 1552, 1537, -1, 1577, 1530, 1552, -1, 1577, 1536, 1530, -1, 1536, 1535, 1530, -1, 1491, 1563, 1548, -1, 1546, 1563, 1491, -1, 1548, 1566, 1549, -1, 1550, 1548, 1547, -1, 1491, 1548, 1550, -1, 1578, 1554, 1505, -1, 1578, 1553, 1554, -1, 1578, 1505, 1553, -1, 1566, 1563, 1560, -1, 1548, 1563, 1566, -1, 1564, 1560, 1561, -1, 1564, 1566, 1560, -1 ] } } ] } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 0.000 description "top" position 14.500 0.000 155.701 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 3.142 description "bottom" position 14.500 0.000 -108.701 } Viewpoint { jump TRUE orientation 1.000 0.000 0.000 1.571 description "front" position 14.500 -132.201 23.500 } Viewpoint { jump TRUE orientation -0.000 -0.707 -0.707 3.142 description "back" position 14.500 132.201 23.500 } Viewpoint { jump TRUE orientation 0.577 -0.577 -0.577 2.094 description "right" position -117.701 0.000 23.500 } Viewpoint { jump TRUE orientation 0.577 0.577 0.577 2.094 description "left" position 146.701 0.000 23.500 } NavigationInfo { avatarSize [0.25, 1.6, 0.75] headlight TRUE speed 1.0 type "EXAMINE" visibilityLimit 0.0 } choreonoid-1.1.0+dfsg/share/models/misc/000077500000000000000000000000001207742442300201065ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/models/misc/floor.wrl000066400000000000000000000067571207742442300217740ustar00rootroot00000000000000#VRML V2.0 utf8 PROTO Joint [ exposedField SFVec3f center 0 0 0 exposedField MFNode children [] exposedField MFFloat llimit [] exposedField MFFloat lvlimit [] exposedField SFRotation limitOrientation 0 0 1 0 exposedField SFString name "" exposedField SFRotation rotation 0 0 1 0 exposedField SFVec3f scale 1 1 1 exposedField SFRotation scaleOrientation 0 0 1 0 exposedField MFFloat stiffness [ 0 0 0 ] exposedField SFVec3f translation 0 0 0 exposedField MFFloat ulimit [] exposedField MFFloat uvlimit [] exposedField SFString jointType "" exposedField SFInt32 jointId -1 exposedField SFVec3f jointAxis 0 0 1 exposedField SFFloat gearRatio 1 exposedField SFFloat rotorInertia 0 exposedField SFFloat rotorResistor 0 exposedField SFFloat torqueConst 1 exposedField SFFloat encoderPulse 1 ] { Transform { center IS center children IS children rotation IS rotation scale IS scale scaleOrientation IS scaleOrientation translation IS translation } } PROTO Segment [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 exposedField SFVec3f centerOfMass 0 0 0 exposedField MFNode children [ ] exposedField SFNode coord NULL exposedField MFNode displacers [ ] exposedField SFFloat mass 0 exposedField MFFloat momentsOfInertia [ 0 0 0 0 0 0 0 0 0 ] exposedField SFString name "" eventIn MFNode addChildren eventIn MFNode removeChildren ] { Group { addChildren IS addChildren bboxCenter IS bboxCenter bboxSize IS bboxSize children IS children removeChildren IS removeChildren } } PROTO Humanoid [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 exposedField SFVec3f center 0 0 0 exposedField MFNode humanoidBody [ ] exposedField MFString info [ ] exposedField MFNode joints [ ] exposedField SFString name "" exposedField SFRotation rotation 0 0 1 0 exposedField SFVec3f scale 1 1 1 exposedField SFRotation scaleOrientation 0 0 1 0 exposedField MFNode segments [ ] exposedField MFNode sites [ ] exposedField SFVec3f translation 0 0 0 exposedField SFString version "1.1" exposedField MFNode viewpoints [ ] ] { Transform { bboxCenter IS bboxCenter bboxSize IS bboxSize center IS center rotation IS rotation scale IS scale scaleOrientation IS scaleOrientation translation IS translation children [ Group { children IS viewpoints } Group { children IS humanoidBody } ] } } DEF Floor Humanoid { humanoidBody [ DEF BASE Joint { jointType "fixed" translation 0.0 0.0 -0.1 rotation 0 0 1 0 children [ DEF BASE_LINK Segment { mass 0.5 momentsOfInertia [ 1 0 0 0 1 0 0 0 1 ] children [ Shape { geometry Box { size 10.0 10.0 0.2 } appearance Appearance { material Material { diffuseColor 0.0 0.0 1.0 } } } ] } ] } ] joints [ USE BASE ] segments [ USE BASE_LINK ] } choreonoid-1.1.0+dfsg/share/models/misc/smallfloor.wrl000066400000000000000000000067571207742442300230250ustar00rootroot00000000000000#VRML V2.0 utf8 PROTO Joint [ exposedField SFVec3f center 0 0 0 exposedField MFNode children [] exposedField MFFloat llimit [] exposedField MFFloat lvlimit [] exposedField SFRotation limitOrientation 0 0 1 0 exposedField SFString name "" exposedField SFRotation rotation 0 0 1 0 exposedField SFVec3f scale 1 1 1 exposedField SFRotation scaleOrientation 0 0 1 0 exposedField MFFloat stiffness [ 0 0 0 ] exposedField SFVec3f translation 0 0 0 exposedField MFFloat ulimit [] exposedField MFFloat uvlimit [] exposedField SFString jointType "" exposedField SFInt32 jointId -1 exposedField SFVec3f jointAxis 0 0 1 exposedField SFFloat gearRatio 1 exposedField SFFloat rotorInertia 0 exposedField SFFloat rotorResistor 0 exposedField SFFloat torqueConst 1 exposedField SFFloat encoderPulse 1 ] { Transform { center IS center children IS children rotation IS rotation scale IS scale scaleOrientation IS scaleOrientation translation IS translation } } PROTO Segment [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 exposedField SFVec3f centerOfMass 0 0 0 exposedField MFNode children [ ] exposedField SFNode coord NULL exposedField MFNode displacers [ ] exposedField SFFloat mass 0 exposedField MFFloat momentsOfInertia [ 0 0 0 0 0 0 0 0 0 ] exposedField SFString name "" eventIn MFNode addChildren eventIn MFNode removeChildren ] { Group { addChildren IS addChildren bboxCenter IS bboxCenter bboxSize IS bboxSize children IS children removeChildren IS removeChildren } } PROTO Humanoid [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 exposedField SFVec3f center 0 0 0 exposedField MFNode humanoidBody [ ] exposedField MFString info [ ] exposedField MFNode joints [ ] exposedField SFString name "" exposedField SFRotation rotation 0 0 1 0 exposedField SFVec3f scale 1 1 1 exposedField SFRotation scaleOrientation 0 0 1 0 exposedField MFNode segments [ ] exposedField MFNode sites [ ] exposedField SFVec3f translation 0 0 0 exposedField SFString version "1.1" exposedField MFNode viewpoints [ ] ] { Transform { bboxCenter IS bboxCenter bboxSize IS bboxSize center IS center rotation IS rotation scale IS scale scaleOrientation IS scaleOrientation translation IS translation children [ Group { children IS viewpoints } Group { children IS humanoidBody } ] } } DEF Floor Humanoid { humanoidBody [ DEF BASE Joint { jointType "fixed" translation 0.0 0.0 -0.01 rotation 0 0 1 0 children [ DEF BASE_LINK Segment { mass 0.5 momentsOfInertia [ 1 0 0 0 1 0 0 0 1 ] children [ Shape { geometry Box { size 2.0 2.0 0.02 } appearance Appearance { material Material { diffuseColor 0.0 0.0 1.0 } } } ] } ] } ] joints [ USE BASE ] segments [ USE BASE_LINK ] } choreonoid-1.1.0+dfsg/share/projects/000077500000000000000000000000001207742442300175215ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/projects/GR001Sample.cnoid000066400000000000000000000131711207742442300224350ustar00rootroot00000000000000items: id: 0 name: "Root" plugin: Base class: RootItem children: - id: 1 name: "World" plugin: Body class: WorldItem data: collisionDetection: true children: - id: 2 name: "Floor" plugin: Body class: BodyItem data: modelFile: "${SHARE}/models/misc/smallfloor.wrl" currentBaseLink: "" rootPosition: [ 0.000000, 0.000000, -0.010000 ] rootAttitude: [ 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000 ] jointPositions: [ ] selfCollisionDetection: true - id: 3 name: "GR001" plugin: Body class: BodyItem data: modelFile: "${SHARE}/models/GR001/GR001.yaml" currentBaseLink: "L_ANKLE_R" rootPosition: [ -0.002066, 0.000207, 0.154197 ] rootAttitude: [ 1.000000, 0.000000, 0.000000, -0.000000, 1.000000, 0.000000, -0.000000, -0.000000, 1.000000 ] jointPositions: [ 0.000000, 0.001970, 0.370928, -0.701190, -0.330262, 0.001970, -0.000000, 0.001970, -0.370928, 0.701190, 0.330262, 0.001970, 0.000000, 0.000000, 0.349066, 0.000000, -0.349066, -0.349066, 0.000000, 0.349066 ] selfCollisionDetection: true children: - id: 4 name: "GRobotController" plugin: GRobot class: GRobotControllerItem data: port: /dev/ttyUSB0 - id: 5 name: "SampleMotion1" plugin: PoseSeq class: PoseSeqItem data: filename: "GR001SampleMotion1.pseq" format: POSE-SEQ-YAML - id: 6 name: "DynamicsSimulator" plugin: Body class: DynamicsSimulatorItem data: staticFriction: 1 slipFriction: 1 views: "Items": selected: [ 5, 3 ] checked: [ 3, 4 ] expanded: [ 1, 3, 4, 5 ] "Scene": mode: view floorGird: true collisions: true shadow: false floorGridSpan: 2 floorGridInterval: 0.05 hiPriorityRendering: false camera: projection: perspetive perspective: [ 40, 1.38023, 0.000802216, 1.63652 ] ortho: [ -1.8953, 1.8953, -1.31263, 1.31263, -2.12922, 2.50346 ] eye: [ 0.517275, 0.0551085, 0.224358 ] center: [ -0.461814, -0.0171288, 0.0341825 ] up: [ -0.18966, -0.0139933, 0.98175 ] "Media": keepAspectRatio: true keepOriginalSize: true "Links": listingMode: "link list" currentBodyItem: 3 bodyItems: - id: 3 selectedLinks: [ 15, 16, 17, 18, 19, 20 ] "Body / Link": showRotationMatrix: false "Joint Sliders": showAllJoints: true jointId: true name: true numColumns: 1 spinBox: true slider: true labelOnLeft: true currentBodyItem: 3 "Multi Value Seq": mode: view editMode: freeLine original: true velocity: false acceleration: false limits: true grid: true gridWidth: 0.2 gridHeight: 0.2 lineWidth: 1 rulers: false sync: true controlPointStep: 1 controlPointOffset: 0 controlPointHeighlight: false scrollMode: continuous lower: -10 upper: 10 "Multi Affine3 Seq": mode: view editMode: freeLine original: true velocity: false acceleration: false limits: true grid: true gridWidth: 0.2 gridHeight: 0.2 lineWidth: 1 rulers: false sync: true controlPointStep: 1 controlPointOffset: 0 controlPointHeighlight: false scrollMode: continuous lower: -10 upper: 10 visibleElements: [ 0, 1, 2 ] "Pose Roll": currentPoseSeqItem: 5 defaultTransitionTime: 0 updateAll: true autoUpdate: false timeSync: true listingMode: "part tree" bodyItems: - id: 3 expandedParts: [ "LEGS", "LOWER-BODY", "Whole Body" ] timeLength: 10 showLipSync: false gridInterval: 1 toolbars: "TimeBar": minTime: 0 maxTime: 30 frameRate: 50 currentTime: 0 "KinematicsBar": mode: AUTO attitude: true penetrationBlock: true collisionLinkHighlight: true snapDistance: 0.025 penetrationBlockDepth: 0.0001 lazyCollisionDetectionMode: true "BodyBar": current: 3 stanceWidth: 0.15 "BodyMotionGenerationBar": balancer: true autoGeneration: true timeScaleRatio: 1 preInitialDuration: 1 postFinalDuration: 1 onlyTimeBarRange: true makeNewBodyItem: true stealthyStepMode: false stealthyHeightRatioThresh: 2 flatLiftingHeight: 0.005 flatLandingHeight: 0.005 impactReductionHeight: 0.005 impactReductionTime: 0.04 autoZmp: true minZmpTransitionTime: 0.1 zmpCenteringTimeThresh: 0.03 zmpTimeMarginBeforeLiftingSpin: 0 allLinkPositions: true lipSyncMix: false Body: "KinematicFaultChecker": checkJointPositions: true angleMargin: 0 translationMargin: 0 checkJointVelocities: true velocityLimitRatio: 100 targetJoints: all checkSelfCollisions: true onlyTimeBarRange: false "SceneBodyManager": sceneBodies: - bodyItem: 2 editable: false centerOfMass: false zmp: false - bodyItem: 3 editable: true centerOfMass: true zmp: true currentFileDialogDirectory: "" choreonoid-1.1.0+dfsg/share/projects/GR001SampleMotion1.pseq000066400000000000000000000307141207742442300235620ustar00rootroot00000000000000# Body pose sequence format version 1.0 defined by cnoid-Robotics type: PoseSeq name: "SampleMotion1" targetBody: "GR001" refs: - time: 0 refer: type: Pose name: "" joints: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ -1.80616593e-19, 0.00197016157, 0.370920524, -0.701176708, -0.330256184, 0.00197016157, -6.05607271e-19, 0.00197016157, -0.370920524, 0.701176708, 0.330256184, 0.00197016157, 0, 0, 0.34906585, 0, -0.34906585, -0.34906585, 0, 0.34906585 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00206589105, 0.000206960048, 0.154197111 ] rotation: [ 1, 7.25663958e-19, 7.30074269e-16, -4.21994409e-19, 1, 3.56094663e-15, -5.75440303e-16, -3.56007164e-15, 1 ] - name: R_ANKLE_R index: 6 translation: [ 0.0252, -0.0196, 0.0211500007 ] rotation: [ 1, 2.24612932e-19, 3.84078922e-16, 1.03126261e-19, 1, 8.67361738e-19, -2.21312768e-16, 7.62329653e-21, 1 ] isTouching: true partingDirection: [ 0, 0, 1 ] - name: L_ANKLE_R index: 12 translation: [ 0.0252, 0.0196, 0.0211500007 ] rotation: [ 1, -8.37197087e-19, -3.70579372e-16, 1.17298181e-18, 1, 6.37075926e-16, 5.86146799e-16, -6.35548514e-16, 1 ] isTouching: true partingDirection: [ 0, 0, 1 ] zmp: [ 0.0202, -5.67254577e-16, 0 ] isZmpStationaryPoint: true - time: 1.5 maxTransitionTime: 0.541666667 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0, 0, -0.172787596, -1.5271631, -0.802851456, 0.172787596, 1.5271631, 0.34906585 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00170177353, 0.000206686534, 0.154270653 ] rotation: [ 0.989016678, -1.0618018e-05, -0.147803962, 1.05159591e-05, 1, -1.4719432e-06, 0.147803962, -9.85240516e-08, 0.989016678 ] - time: 2.46666667 maxTransitionTime: 0.558333333 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0.00263595484, 0.354301838, -0.202445604, -1.65626266, -0.549886095, -0.62066345, 0.667445809, 1.41804495 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00231165222, 0.000286113927, 0.154157425 ] rotation: [ 0.854382857, 0.344858111, 0.388719458, -0.342913079, 0.936216263, -0.0768747615, -0.390436363, -0.0676165079, 0.918143482 ] - time: 2.99166667 maxTransitionTime: 0.341666667 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0.00263653005, 0, 0.205694412, -1.16597616, -0.98031669, -0.212686887, 1.15757596, 0.987788984 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ 0.00557454746, -2.64736876e-05, 0.143289054 ] rotation: [ 0.914441672, 0.00288814826, 0.40470741, -5.23028162e-05, 0.999975372, -0.00701803127, -0.404717712, 0.00639641291, 0.914419302 ] - time: 3.48333333 maxTransitionTime: 0.516666667 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0.00263595492, -0.422369679, 0.620629366, -0.667486757, -1.418009, 0.202411496, 1.65622169, 0.549922076 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ 0.0141276682, -0.000322323269, 0.150963912 ] rotation: [ 0.840461288, -0.342483373, 0.419916614, 0.343579605, 0.936061362, 0.0757771775, -0.419020141, 0.0805870002, 0.90439364 ] - time: 4.00833333 maxTransitionTime: 0.341666667 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0.00263653005, 0, 0.205694412, -1.16597616, -0.98031669, -0.212686887, 1.15757596, 0.987788984 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ 0.00557454746, -2.64736876e-05, 0.143289054 ] rotation: [ 0.914441672, 0.00288814826, 0.40470741, -5.23028162e-05, 0.999975372, -0.00701803127, -0.404717712, 0.00639641291, 0.914419302 ] - time: 4.49166667 maxTransitionTime: 0.541666667 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0, 0, -0.172787596, -1.5271631, -0.802851456, 0.172787596, 1.5271631, 0.34906585 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00170177353, 0.000206686534, 0.154270653 ] rotation: [ 0.989016678, -1.0618018e-05, -0.147803962, 1.05159591e-05, 1, -1.4719432e-06, 0.147803962, -9.85240516e-08, 0.989016678 ] - time: 5.025 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0, 0.326291052, 0.942000489, -0.906487926, -1.25535486, 1.46282615, 0.771257023, 1.18819094 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00595981699, 0.00698479091, 0.147243988 ] rotation: [ 0.999982472, -0.00179845846, -0.00564107445, 1.07496363e-05, 0.953301255, -0.302021056, 0.00592081567, 0.302015701, 0.953284564 ] - time: 5.525 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0, 9.72347334e-05, 0.581302161, -1.03668676, -0.92861256, 1.28636026, 0.834248605, 0.836117661 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00576741128, -0.000178572423, 0.15122398 ] rotation: [ 0.993234316, 0.00517775005, -0.116011999, -9.85958498e-05, 0.999042755, 0.0437443105, 0.116127444, -0.043436912, 0.992284058 ] - time: 6.01666667 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0, 0.326291052, 0.942000489, -0.906487926, -1.25535486, 1.46282615, 0.771257023, 1.18819094 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00595981699, 0.00698479091, 0.147243988 ] rotation: [ 0.999982472, -0.00179845846, -0.00564107445, 1.07496363e-05, 0.953301255, -0.302021056, 0.00592081567, 0.302015701, 0.953284564 ] - time: 6.54166667 maxTransitionTime: 0.541666667 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0, 9.84430804e-06, -0.172753962, -1.52714437, -0.802865108, 0.172826517, 1.52714029, 0.349091167 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.000709439304, 0.0272837379, 0.14415081 ] rotation: [ 0.989019285, -0.000140269134, -0.147786448, 1.26891509e-05, 0.999999626, -0.000864215013, 0.147786514, 0.00085285003, 0.989018917 ] - time: 7.15871683 maxTransitionTime: 0.724353292 refer: type: Pose name: "" joints: [ 0, 1, 2, 3, 4, 5, 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ -0.00448554345, 0.158707861, 0.0575528145, -0.87765158, -0.254806122, 0.0113587088, 0, -0.387463094, -0.888372589, -1.30899694, -1.55159771, -0.750491578, 1.18333323, 0.900589894 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00218358137, 0.0359159308, 0.154265647 ] rotation: [ 0.999850594, 0.0169426471, 0.0034257387, -0.0172852919, 0.979002044, 0.203116263, 8.75219726e-05, -0.203145131, 0.979148634 ] - name: R_ANKLE_R index: 6 translation: [ -0.0143382891, -0.0216689863, 0.0441276821 ] rotation: [ 0.840607085, 0.0267631026, 0.540983794, -0.208611213, 0.937726404, 0.277759883, -0.499861072, -0.346342211, 0.793842542 ] - time: 7.83107165 maxTransitionTime: 0.328014784 refer: type: Pose name: "" joints: [ 0, 1, 2, 3, 4, 5, 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ -0.130863974, 0.227028823, 0.903551924, -0.934143105, -0.153307758, -0.0196133095, 0, -0.387463094, -0.125452629, -0.916723361, -1.23611023, -1.00903081, 1.10115248, 0.800432771 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00632375134, 0.0370071214, 0.154259643 ] rotation: [ 0.981132548, -0.13053257, 0.142618974, 0.0973393867, 0.970867715, 0.218954159, -0.167044806, -0.200940609, 0.965255875 ] - name: R_ANKLE_R index: 6 translation: [ 0.043821894, -0.0304316716, 0.0390835821 ] rotation: [ 0.999304947, -0.0337694092, 0.0157876539, 0.0230297452, 0.892288296, 0.450878284, -0.0293130321, -0.450201314, 0.892445809 ] - time: 8.18024868 refer: type: Pose name: "" joints: [ 0, 1, 2, 3, 4, 5, 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ -0.13240401, 0.285252766, 1.26631178, -0.945796389, -0.282969297, 0.0350767145, 0, -0.387463094, 0.483781988, -0.727534958, -0.947549306, -1.50969822, 0.960895056, 0.454597378 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.0069119989, 0.0368694778, 0.154226214 ] rotation: [ 0.97293155, -0.114419492, 0.200779429, 0.0687961848, 0.972838195, 0.221026994, -0.220615694, -0.201231277, 0.954376597 ] - name: R_ANKLE_R index: 6 translation: [ 0.0664605398, -0.030431704, 0.0496545353 ] rotation: [ 0.918242056, -0.0558642713, -0.392059575, 0.226221579, 0.886569969, 0.403506489, 0.325046649, -0.459208964, 0.826723535 ] - time: 9.01615732 maxTransitionTime: 0.529056104 refer: type: Pose name: "" joints: [ 0, 1, 2, 3, 4, 5, 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ -0.000871297291, 0.29927517, 0.52329174, -0.953265731, -0.43274688, 0.298156892, 0, -0.00173207729, 0.349668072, -0.37533689, -0.351464137, -0.370906241, 0.389208423, 0.349380348 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00287269103, 0.0297707957, 0.144534584 ] rotation: [ 0.999999465, -0.00028591801, 0.000994134096, 0.000284994235, 0.999999528, 0.000929243977, -0.000994399314, -0.000928960157, 0.999999074 ] - name: R_ANKLE_R index: 6 translation: [ 0.0253871799, -0.0193565247, 0.0212777166 ] rotation: [ 0.999998603, -0.000230293824, -0.00165575874, 0.000233684636, 0.999997876, 0.0020479879, 0.00165528359, -0.00204837197, 0.999996532 ] isTouching: true partingDirection: [ 0, 0, 1 ] - time: 9.65102465 maxTransitionTime: 0.677191813 refer: type: Pose name: "" joints: [ 12, 13, 14, 15, 16, 17, 18, 19 ] q: [ 0, 0, 0.34906585, 0, -0.34906585, -0.34906585, 0, 0.34906585 ] ikLinks: - name: WAIST index: 0 isBaseLink: true translation: [ -0.00206589105, 0.000206960048, 0.154197111 ] rotation: [ 1, 7.25663958e-19, 7.30074269e-16, -4.21994409e-19, 1, 3.56094663e-15, -5.75440303e-16, -3.56007164e-15, 1 ] choreonoid-1.1.0+dfsg/share/sampleplugins/000077500000000000000000000000001207742442300205535ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/sampleplugins/GLGearsPlugin/000077500000000000000000000000001207742442300232165ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/sampleplugins/GLGearsPlugin/GLGearsPlugin.cpp000066400000000000000000000005151207742442300263660ustar00rootroot00000000000000 #include "GLGearsView.h" #include using namespace cnoid; class GLGearsPlugin : public Plugin { public: GLGearsPlugin() : Plugin("GLGears") { } virtual bool initialize() { addView(new GLGearsView()); return true; } }; CNOID_IMPLEMENT_PLUGIN_ENTRY(GLGearsPlugin); choreonoid-1.1.0+dfsg/share/sampleplugins/GLGearsPlugin/GLGearsView.cpp000066400000000000000000000171571207742442300260540ustar00rootroot00000000000000/* The base program of this sample is the "3-D gear wheels" sample of gtkglextmm. The origiral "3-D gear wheels" is a public domain program written by Brian Paul and its conversion to gtkglextmm was done by Naofumi Yasufuku. */ #include "GLGearsView.h" #include #include using namespace std; using namespace boost; using namespace cnoid; GearsScene::GearsScene(QWidget* parent) : QGLWidget(parent), gear1(0), gear2(0), gear3(0), viewRotX(20.0), viewRotY(30.0), viewRotZ(0.0), angle(0.0) { } bool GearsScene::setTime(double time) { if(time < 30.0){ angle = time * M_PI * 4.0; update(); return true; } return false; } void GearsScene::initializeGL() { static GLfloat pos[4] = {5.0, 5.0, 10.0, 0.0}; static GLfloat red[4] = {0.8, 0.1, 0.0, 1.0}; static GLfloat green[4] = {0.0, 0.8, 0.2, 1.0}; static GLfloat blue[4] = {0.2, 0.2, 1.0, 1.0}; glLightfv(GL_LIGHT0, GL_POSITION, pos); glEnable(GL_CULL_FACE); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_DEPTH_TEST); // Make the gears. gear1 = glGenLists(1); glNewList(gear1, GL_COMPILE); glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, red); gear(1.0, 4.0, 1.0, 20, 0.7); glEndList(); gear2 = glGenLists(1); glNewList(gear2, GL_COMPILE); glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, green); gear(0.5, 2.0, 2.0, 10, 0.7); glEndList(); gear3 = glGenLists(1); glNewList(gear3, GL_COMPILE); glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, blue); gear(1.3, 2.0, 0.5, 10, 0.7); glEndList(); glEnable(GL_NORMALIZE); } void GearsScene::resizeGL(int width, int height) { GLfloat h = (GLfloat)height / width; glViewport(0, 0, width, height); glMatrixMode(GL_PROJECTION); glLoadIdentity(); glFrustum(-1.0, 1.0, -h, h, 5.0, 60.0); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glTranslatef(0.0, 0.0, -40.0); } void GearsScene::paintGL() { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glPushMatrix(); glRotatef(viewRotX, 1.0, 0.0, 0.0); glRotatef(viewRotY, 0.0, 1.0, 0.0); glRotatef(viewRotZ, 0.0, 0.0, 1.0); glPushMatrix(); glTranslatef(-3.0, -2.0, 0.0); glRotatef(angle, 0.0, 0.0, 1.0); glCallList(gear1); glPopMatrix(); glPushMatrix(); glTranslatef(3.1, -2.0, 0.0); glRotatef(-2.0 * angle - 9.0, 0.0, 0.0, 1.0); glCallList(gear2); glPopMatrix(); glPushMatrix(); glTranslatef(-3.1, 4.2, 0.0); glRotatef(-2.0 * angle - 25.0, 0.0, 0.0, 1.0); glCallList(gear3); glPopMatrix(); glPopMatrix(); } /* * Draw a gear wheel. You'll probably want to call this function when * building a display list since we do a lot of trig here. * * Input: inner_radius - radius of hole at center * outer_radius - radius at center of teeth * width - width of gear * teeth - number of teeth * tooth_depth - depth of tooth */ void GearsScene::gear(GLfloat innerRadius, GLfloat outerRadius, GLfloat width, GLint teeth, GLfloat toothDepth) { GLint i; GLfloat r0, r1, r2; GLfloat angle, da; GLfloat u, v, len; r0 = innerRadius; r1 = outerRadius - toothDepth / 2.0; r2 = outerRadius + toothDepth / 2.0; da = 2.0 * M_PI / teeth / 4.0; glShadeModel(GL_FLAT); glNormal3f(0.0, 0.0, 1.0); /* draw front face */ glBegin(GL_QUAD_STRIP); for(i = 0; i <= teeth; i++){ angle = i * 2.0 * M_PI / teeth; glVertex3f(r0 * cos(angle), r0 * sin(angle), width * 0.5); glVertex3f(r1 * cos(angle), r1 * sin(angle), width * 0.5); if(i < teeth) { glVertex3f(r0 * cos(angle), r0 * sin(angle), width * 0.5); glVertex3f(r1 * cos(angle + 3 * da), r1 * sin(angle + 3 * da), width * 0.5); } } glEnd(); /* draw front sides of teeth */ glBegin(GL_QUADS); da = 2.0 * M_PI / teeth / 4.0; for(i = 0; i < teeth; i++){ angle = i * 2.0 * M_PI / teeth; glVertex3f(r1 * cos(angle), r1 * sin(angle), width * 0.5); glVertex3f(r2 * cos(angle + da), r2 * sin(angle + da), width * 0.5); glVertex3f(r2 * cos(angle + 2 * da), r2 * sin(angle + 2 * da), width * 0.5); glVertex3f(r1 * cos(angle + 3 * da), r1 * sin(angle + 3 * da), width * 0.5); } glEnd(); glNormal3f(0.0, 0.0, -1.0); /* draw back face */ glBegin(GL_QUAD_STRIP); for (i = 0; i <= teeth; i++) { angle = i * 2.0 * M_PI / teeth; glVertex3f(r1 * cos(angle), r1 * sin(angle), -width * 0.5); glVertex3f(r0 * cos(angle), r0 * sin(angle), -width * 0.5); if (i < teeth) { glVertex3f(r1 * cos(angle + 3 * da), r1 * sin(angle + 3 * da), -width * 0.5); glVertex3f(r0 * cos(angle), r0 * sin(angle), -width * 0.5); } } glEnd(); /* draw back sides of teeth */ glBegin(GL_QUADS); da = 2.0 * M_PI / teeth / 4.0; for (i = 0; i < teeth; i++) { angle = i * 2.0 * M_PI / teeth; glVertex3f(r1 * cos(angle + 3 * da), r1 * sin(angle + 3 * da), -width * 0.5); glVertex3f(r2 * cos(angle + 2 * da), r2 * sin(angle + 2 * da), -width * 0.5); glVertex3f(r2 * cos(angle + da), r2 * sin(angle + da), -width * 0.5); glVertex3f(r1 * cos(angle), r1 * sin(angle), -width * 0.5); } glEnd(); /* draw outward faces of teeth */ glBegin(GL_QUAD_STRIP); for(i = 0; i < teeth; i++){ angle = i * 2.0 * M_PI / teeth; glVertex3f(r1 * cos(angle), r1 * sin(angle), width * 0.5); glVertex3f(r1 * cos(angle), r1 * sin(angle), -width * 0.5); u = r2 * cos(angle + da) - r1 * cos(angle); v = r2 * sin(angle + da) - r1 * sin(angle); len = sqrt(u * u + v * v); u /= len; v /= len; glNormal3f(v, -u, 0.0); glVertex3f(r2 * cos(angle + da), r2 * sin(angle + da), width * 0.5); glVertex3f(r2 * cos(angle + da), r2 * sin(angle + da), -width * 0.5); glNormal3f(cos(angle), sin(angle), 0.0); glVertex3f(r2 * cos(angle + 2 * da), r2 * sin(angle + 2 * da), width * 0.5); glVertex3f(r2 * cos(angle + 2 * da), r2 * sin(angle + 2 * da), -width * 0.5); u = r1 * cos(angle + 3 * da) - r2 * cos(angle + 2 * da); v = r1 * sin(angle + 3 * da) - r2 * sin(angle + 2 * da); glNormal3f(v, -u, 0.0); glVertex3f(r1 * cos(angle + 3 * da), r1 * sin(angle + 3 * da), width * 0.5); glVertex3f(r1 * cos(angle + 3 * da), r1 * sin(angle + 3 * da), -width * 0.5); glNormal3f(cos(angle), sin(angle), 0.0); } glVertex3f(r1 * cos(0.0), r1 * sin(0.0), width * 0.5); glVertex3f(r1 * cos(0.0), r1 * sin(0.0), -width * 0.5); glEnd(); glShadeModel(GL_SMOOTH); /* draw inside radius cylinder */ glBegin(GL_QUAD_STRIP); for(i = 0; i <= teeth; i++){ angle = i * 2.0 * M_PI / teeth; glNormal3f(-cos(angle), -sin(angle), 0.0); glVertex3f(r0 * cos(angle), r0 * sin(angle), -width * 0.5); glVertex3f(r0 * cos(angle), r0 * sin(angle), width * 0.5); } glEnd(); } GLGearsView::GLGearsView() : gearsScene(false) { setName("Gears"); gearsScene = new GearsScene(this); QVBoxLayout* vbox = new QVBoxLayout(); vbox->addWidget(gearsScene); setLayout(vbox); timeBar = TimeBar::instance(); } void GLGearsView::onActivated() { if(!timeChangeConnection.connected()){ timeChangeConnection = timeBar->sigTimeChanged().connect( bind(&GearsScene::setTime, gearsScene, _1)); } gearsScene->setTime(timeBar->time()); } void GLGearsView::onDeactivated() { if(timeChangeConnection.connected()){ timeChangeConnection.disconnect(); } } choreonoid-1.1.0+dfsg/share/sampleplugins/GLGearsPlugin/GLGearsView.h000066400000000000000000000021501207742442300255040ustar00rootroot00000000000000/* The base program of this sample is the "3-D gear wheels" sample of gtkglextmm. The origiral "3-D gear wheels" is a public domain program written by Brian Paul and its conversion to gtkglextmm was wirtten by Naofumi Yasufuku. */ #include #include #include #include class GearsScene : public QGLWidget { public: GearsScene(QWidget* parent = 0); bool setTime(double time); protected: virtual void initializeGL(); virtual void resizeGL(int width, int height); virtual void paintGL(); private: void gear(GLfloat innerRadius, GLfloat outerRadius, GLfloat width, GLint teeth, GLfloat toothDepth); protected: GLint gear1; GLint gear2; GLint gear3; GLfloat viewRotX; GLfloat viewRotY; GLfloat viewRotZ; GLfloat angle; }; class GLGearsView : public cnoid::View { public: GLGearsView(); protected: virtual void onActivated(); virtual void onDeactivated(); private: GearsScene* gearsScene; cnoid::TimeBar* timeBar; boost::signals::connection timeChangeConnection; }; choreonoid-1.1.0+dfsg/share/sampleplugins/GLGearsPlugin/Makefile000066400000000000000000000004651207742442300246630ustar00rootroot00000000000000 CXXFLAGS += `pkg-config --cflags choreonoid` PLUGIN = libCnoidGLGearsPlugin.so SRC = GLGearsPlugin.o GLGearsView.o $(PLUGIN): $(SRC) g++ -shared `pkg-config --libs choreonoid` -o $(PLUGIN) $(SRC) install: $(PLUGIN) install -s $(PLUGIN) `pkg-config --variable=plugindir choreonoid` clean: rm -f *.o *.so choreonoid-1.1.0+dfsg/share/sampleplugins/HelloWorldPlugin/000077500000000000000000000000001207742442300240055ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/sampleplugins/HelloWorldPlugin/HelloWorldPlugin.cpp000066400000000000000000000013511207742442300277430ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include #include #include #include using namespace cnoid; using namespace boost; class HelloWorldPlugin : public Plugin { void onHelloWorldActivated() { MessageView::mainInstance()->putln(QObject::tr("Hello World !")); } public: HelloWorldPlugin() : Plugin("HelloWorld") { } virtual bool initialize() { menuManager().setPath(QObject::tr("/View")).addItem(QObject::tr("Hello World")) ->sigTriggered().connect( bind(&HelloWorldPlugin::onHelloWorldActivated, this)); return true; } }; CNOID_IMPLEMENT_PLUGIN_ENTRY(HelloWorldPlugin) choreonoid-1.1.0+dfsg/share/sampleplugins/HelloWorldPlugin/Makefile000066400000000000000000000004551207742442300254510ustar00rootroot00000000000000 CXXFLAGS += `pkg-config --cflags choreonoid` PLUGIN = libCnoidHelloWorldPlugin.so $(PLUGIN): HelloWorldPlugin.o g++ -shared `pkg-config --libs choreonoid` -o $(PLUGIN) HelloWorldPlugin.o install: $(PLUGIN) install -s $(PLUGIN) `pkg-config --variable=plugindir choreonoid` clean: rm -f *.o *.so choreonoid-1.1.0+dfsg/share/sampleplugins/Sample1Plugin/000077500000000000000000000000001207742442300232345ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/share/sampleplugins/Sample1Plugin/Makefile000066400000000000000000000004771207742442300247040ustar00rootroot00000000000000 CXXFLAGS += `pkg-config --cflags choreonoid-body-plugin` PLUGIN = libCnoidSample1Plugin.so SRC = Sample1Plugin.o $(PLUGIN): $(SRC) g++ -shared `pkg-config --libs choreonoid-body-plugin` -o $(PLUGIN) $(SRC) install: $(PLUGIN) install -s $(PLUGIN) `pkg-config --variable=plugindir choreonoid` clean: rm -f *.o *.so choreonoid-1.1.0+dfsg/share/sampleplugins/Sample1Plugin/Sample1Plugin.cpp000066400000000000000000000021621207742442300264220ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include #include #include #include #include using namespace boost; using namespace cnoid; void onButtonClicked(double dq) { ItemList bodyItems = ItemTreeView::mainInstance()->selectedItems(); for(size_t i=0; i < bodyItems.size(); ++i){ BodyPtr body = bodyItems[i]->body(); for(int j=0; j < body->numJoints(); ++j){ body->joint(j)->q += dq; } bodyItems[i]->notifyKinematicStateChange(true); } } class Sample1Plugin : public Plugin { public: Sample1Plugin() : Plugin("Sample1") { require("Body"); } virtual bool initialize() { ToolBar* bar = new ToolBar("Sample1"); bar->addButton("Increment") ->sigClicked().connect(bind(&onButtonClicked, +0.04)); bar->addButton("Decrement") ->sigClicked().connect(bind(&onButtonClicked, -0.04)); addToolBar(bar); return true; } }; CNOID_IMPLEMENT_PLUGIN_ENTRY(Sample1Plugin) choreonoid-1.1.0+dfsg/src/000077500000000000000000000000001207742442300153555ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Base/000077500000000000000000000000001207742442300162275ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Base/Action.cpp000066400000000000000000000014741207742442300201560ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "Action.h" using namespace cnoid; Action::Action(QObject* parent) : QAction(parent) { initialize(); } Action::Action(const QString& text, QObject* parent) : QAction(text, parent) { initialize(); } Action::Action(const QIcon& icon, QObject* parent) : QAction(parent) { setIcon(icon); initialize(); } Action::Action(const QIcon& icon, const QString& text, QObject* parent) : QAction(icon, text, parent) { initialize(); } void Action::initialize() { connect(this, SIGNAL(triggered(bool)), this, SLOT(onTriggered(bool))); connect(this, SIGNAL(toggled(bool)), this, SLOT(onToggled(bool))); } void Action::onTriggered(bool checked) { sigTriggered_(); } void Action::onToggled(bool checked) { sigToggled_(checked); } choreonoid-1.1.0+dfsg/src/Base/Action.h000066400000000000000000000020211207742442300176100ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_ACTION_H_INCLUDED #define CNOID_GUIBASE_ACTION_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT Action : public QAction { Q_OBJECT public: Action(QObject* parent); Action(const QString& text, QObject* parent); Action(const QIcon& icon, QObject* parent); Action(const QIcon& icon, const QString& text, QObject* parent); inline SignalProxy< boost::signal > sigTriggered() { return sigTriggered_; } inline SignalProxy< boost::signal > sigToggled() { return sigToggled_; } private Q_SLOTS: void onTriggered(bool checked); void onToggled(bool checked); private: boost::signal sigTriggered_; boost::signal sigToggled_; void initialize(); }; } #endif choreonoid-1.1.0+dfsg/src/Base/App.cpp000066400000000000000000000222451207742442300174600ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "App.h" #include "AppConfig.h" #include "PluginManager.h" #include "ExtensionManager.h" #include "ItemManager.h" #include "ProjectManager.h" #include "MenuManager.h" #include "OptionManager.h" #include "TimeSyncItemEngineManager.h" #include "MainWindow.h" #include "RootItem.h" #include "FolderItem.h" #include "View.h" #include "MessageView.h" #include "ItemTreeView.h" #include "ItemPropertyView.h" #include "SceneView.h" #include "TimeBar.h" #include "FileBar.h" #include "GraphBar.h" #include "SceneItem.h" #include "MultiValueSeqItem.h" #include "MultiAffine3SeqItem.h" #include "Vector3SeqItem.h" #include "ProjectPathSetEditor.h" #include "DescriptionDialog.h" #include "Licenses.h" //#include "MovieGenerator.h" #include #include #include #include #include #include #include #include #ifdef Q_OS_WIN32 #include #endif #ifdef Q_OS_UNIX #include #endif #ifdef Q_OS_DARWIN #include #endif #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class AppImpl : public QObject { public: AppImpl(App* self, int& argc, char**& argv); ~AppImpl(); App* self; int& argc; char**& argv; PluginManager* pluginManager; ExtensionManager* ext; string appName; string vendorName; DescriptionDialog* descriptionDialog; void initialize(const char* appName, const char* vendorName, const QIcon& icon, const char* pluginPathList); void findProgramTopDirectory(); int exec(); bool processCommandLineOptions(); virtual bool eventFilter(QObject* watched, QEvent* event); void showInformationDialog(); }; } namespace { string programTopDirectory_; string shareDirectory_; string programDirectory_; } App::App(int& argc, char**& argv) : QApplication(argc, argv) { impl = new AppImpl(this, argc, argv); } AppImpl::AppImpl(App* self, int& argc, char**& argv) : self(self), argc(argc), argv(argv) { descriptionDialog = 0; } void App::initialize(const char* appName, const char* vendorName, const QIcon& icon, const char* pluginPathList) { impl->initialize(appName, vendorName, icon, pluginPathList); } void AppImpl::initialize( const char* appName, const char* vendorName, const QIcon& icon, const char* pluginPathList) { this->appName = appName; this->vendorName = vendorName; findProgramTopDirectory(); setlocale(LC_ALL, ""); // for gettext YamlNode::initialize(); QTextCodec::setCodecForCStrings(QTextCodec::codecForLocale()); self->setApplicationName(appName); self->setOrganizationName(vendorName); self->setWindowIcon(icon); ext = new ExtensionManager("Base", false); AppConfig::initialize(appName, vendorName); MainWindow::initialize(appName, ext); MessageView::initialize(ext); RootItem::initialize(ext); ProjectManager::initialize(ext); FileBar::initialize(ext); TimeBar::initialize(ext); ItemTreeView::initialize(ext); ItemPropertyView::initialize(ext); SceneView::initialize(ext); GraphBar::initialize(ext); TimeSyncItemEngineManager::initialize(); FolderItem::initialize(ext); SceneItem::initialize(ext); MultiValueSeqItem::initialize(ext); MultiAffine3SeqItem::initialize(ext); Vector3SeqItem::initialize(ext); ProjectPathSetEditor::initialize(ext); //initializeMovieGenerator(*ext); ext->menuManager().setPath("/Help").addItem(_("About Choreonoid Framework")) ->sigTriggered().connect(bind(&AppImpl::showInformationDialog, this)); pluginManager = PluginManager::instance(); if(pluginPathList){ pluginManager->scanPluginFilesInPathList(pluginPathList); } pluginManager->scanPluginFilesInDirectoyOfExecFile(); pluginManager->loadAllPlugins(); MainWindow::instance()->installEventFilter(this); } App::~App() { if(impl){ delete impl; } } AppImpl::~AppImpl() { AppConfig::flush(); delete pluginManager; } void AppImpl::findProgramTopDirectory() { #ifdef Q_OS_WIN32 static const int BUFSIZE = 1024; TCHAR execFilePath[BUFSIZE]; if(GetModuleFileName(NULL, execFilePath, BUFSIZE)){ #ifndef UNICODE programDirectory_ = filesystem::path(execFilePath).branch_path().file_string(); programTopDirectory_ = filesystem::path(execFilePath).branch_path().branch_path().file_string(); #else int codepage = _getmbcp(); const int newSize = WideCharToMultiByte(codepage, 0, execFilePath, -1, NULL, 0, NULL, NULL); if(newSize > 0){ vector execFilePathMB(newSize + 1); newSize = WideCharToMultiByte(codepage, 0, execFilePath, -1, &execFilePathMB[0], newSize + 1, NULL, NULL); programDirectory_ = filesystem::path(execFilePathUtf8).branch_path().file_string(); programTopDirectory_ = filesystem::path(execFilePathUtf8).branch_path().branch_path().file_string(); } #endif // UNICODE } #endif // Q_OS_WIN32 #ifdef Q_OS_LINUX utsname info; if(uname(&info) == 0){ if(strncmp(info.sysname, "Linux", 6) == 0){ static const int BUFSIZE = 1024; char buf[BUFSIZE]; int n = readlink("/proc/self/exe", buf, BUFSIZE - 1); buf[n] = 0; programDirectory_ = filesystem::path(buf).branch_path().file_string(); programTopDirectory_ = filesystem::path(buf).branch_path().branch_path().file_string(); } } #endif // Q_OS_LINUX #ifdef Q_OS_DARWIN char buf[1024]; uint32_t n = sizeof(buf); if(_NSGetExecutablePath(buf, &n) == 0){ programDirectory_ = filesystem::path(buf).branch_path().file_string(); programTopDirectory_ = filesystem::path(buf).branch_path().branch_path().file_string(); } #endif // Q_OS_DARWIN filesystem::path sharePath = filesystem::path(programTopDirectory_) / CNOID_SHARE_SUBDIR; if(filesystem::is_directory(sharePath)){ shareDirectory_ = sharePath.file_string(); } else { shareDirectory_ = sharePath.parent_path().file_string(); } } const string& App::topDirectory() { return programTopDirectory_; } const std::string& App::shareDirectory() { return shareDirectory_; } const string& App::programDirectory() { return programDirectory_; } int App::exec() { return impl->exec(); } int AppImpl::exec() { MainWindow* mainWindow = MainWindow::instance(); processCommandLineOptions(); if(!mainWindow->isVisible()){ mainWindow->show(); } int result = self->QApplication::exec(); if(pluginManager->unloadAllPlugins()){ delete ext; delete mainWindow; } return result; } bool AppImpl::eventFilter(QObject* watched, QEvent* event) { if(watched == MainWindow::instance()){ if(event->type() == QEvent::Close){ event->accept(); return true; } } return false; } bool AppImpl::processCommandLineOptions() { if(!ext->optionManager().parseCommandLine(argc, argv)){ //put error messages } return false; } void AppImpl::showInformationDialog() { if(!descriptionDialog){ descriptionDialog = new DescriptionDialog(); descriptionDialog->setWindowTitle(_("About Choreonoid Framework")); descriptionDialog->setDescription( QString(_("Choreonoid Framework Version %1\n")).arg(CNOID_FULL_VERSION_STRING) + _("\n" "This program has been developed by Shin'ichiro Nakaoka and Choreonoid Development Team, AIST, " "and is distributed as a part of the Choreonoid package.\n" "\n") + LGPLtext() + _("The source and some binary packages of this program also include the following thirdparty " "libraries:\n" "\n" "* Eigen (http://eigen.tuxfamily.org/)\n" "* IJG JPEG Library (http://www.ijg.org/)\n" "* libpng (http://www.libpng.org/pub/png/libpng.html)\n" "* LibYAML (http://pyyaml.org/wiki/LibYAML)\n" "* zlib (http://zlib.net/)\n" "\n" "These libraries are used and redistributed under the terms of their licenses. Please see " "the corresponding directories in the source package or their official web sites to see the " "details of their licenses.\n" "\n" "This program also depends on a lot of other external libraries, which are linked into this " "program during the execution. Some binary packages of this program may include the binaries " "of the following libraries:\n" "\n" "* Qt (http://qt.nokia.com/)\n" "* OpenSceneGraph (http://www.openscenegraph.org)\n" "* Boost C++ Libraries (http://www.boost.org/) \n" "\n" "Please see their official web sites to see the details of their licenses.\n" )); } descriptionDialog->show(); } choreonoid-1.1.0+dfsg/src/Base/App.h000066400000000000000000000015511207742442300171220ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_APP_H_INCLUDED #define CNOID_GUIBASE_APP_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class ExtensionManager; class AppImpl; class CNOID_EXPORT App : public QApplication { public: /** @if jp @param appName アプリケーションå @param vendorName ベンダå @endif */ App(int& argc, char**& argv); ~App(); void initialize(const char* appName, const char* vendorName, const QIcon& icon, const char* pluginPathList); int exec(); static const std::string& topDirectory(); static const std::string& shareDirectory(); static const std::string& programDirectory(); private: AppImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/AppConfig.cpp000066400000000000000000000066531207742442300206130ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "AppConfig.h" #include "MessageView.h" #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; using boost::filesystem::path; namespace { string application; string organization; path configDirPath; path filePath; path fullPath; shared_ptr pYamlReader; }; bool AppConfig::initialize(const std::string& application_, const std::string& organization_) { application = application_; organization = organization_; #ifdef WIN32 const char* appdata = getenv("APPDATA"); if(appdata){ configDirPath = path(appdata) / path(organization_); } #else const char* home = getenv("HOME"); if(home){ configDirPath = path(home) / path(".config") / path(organization_); } #endif filePath = application + ".conf"; if(!configDirPath.empty()){ fullPath = configDirPath / filePath; std::string fullPathString = fullPath.file_string(); load(fullPathString); } return !fullPath.empty(); } // I’ˆÓ archive()‚Ŏ擾‚µ‚½ƒ|ƒCƒ“ƒ^‚ð•ÛŽ‚·‚éꇂÍAloadŽž‚É“ü‚ê‘Ö‚¦‚邱‚ÆI YamlMapping* AppConfig::archive() { if(pYamlReader && pYamlReader->numDocuments()){ return pYamlReader->document()->toMapping(); } static YamlMappingPtr appArchive(new YamlMapping); return appArchive.get(); } bool AppConfig::flush() { if(configDirPath.empty()){ return false; } if(!filesystem::exists(fullPath)){ if(filesystem::exists(configDirPath)){ if(!filesystem::is_directory(configDirPath)){ const char* m = "\"%1%\" is not a directory.\n" "It should be directory to contain the config file.\n" "The configuration cannot be stored into the file system"; showWarningDialog(format(_(m)) % configDirPath.file_string()); return false; } } else { filesystem::create_directories(configDirPath); } } return save(fullPath.file_string()); } bool AppConfig::save(const std::string& filename) { try { YamlWriter writer(filename); writer.setKeyOrderPreservationMode(true); writer.putNode(archive()); } catch(const YamlNode::Exception& ex){ showWarningDialog(ex.message()); return false; } return true; } /// @note ’ˆÓ! YamlReader‚ª“ü‚ê‘Ö‚í‚é‚Ì‚ÅloadŒã‚Íarchive()‚Ŏ擾‚µ‚½ƒ|ƒCƒ“ƒ^‚ð“ü‚ê‘Ö‚¦‚邱‚ÆI bool AppConfig::load(const std::string& filename) { YamlReader* pyaml = new YamlReader(); boost::filesystem::path filePath(filename); if(filesystem::exists(filePath)){ try { if(pyaml->load(filename)){ if(pyaml->numDocuments() != 1 || pyaml->document()->type() != YAML_MAPPING){ pyaml->clearDocuments(); } } } catch (const YamlNode::Exception& ex){ ostream& os = MessageView::mainInstance()->cout(); os << format("Application config file \"%1%\" cannot be loaded (%2%).") % filename % ex.message() << endl; pyaml->clearDocuments(); delete pyaml; return false; } } pYamlReader = shared_ptr(pyaml); return true; } choreonoid-1.1.0+dfsg/src/Base/AppConfig.h000066400000000000000000000010621207742442300202450ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_APP_CONFIG_H_INCLUDED #define CNOID_GUIBASE_APP_CONFIG_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT AppConfig { public: static bool initialize(const std::string& application, const std::string& organization); static YamlMapping* archive(); static bool flush(); static bool save(const std::string& filename); static bool load(const std::string& filename); }; } #endif choreonoid-1.1.0+dfsg/src/Base/Archive.cpp000066400000000000000000000174211207742442300203210ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "Archive.h" #include "Item.h" #include "App.h" #include "AppConfig.h" #include "MessageView.h" #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { QRegExp regexVar("^\\$\\{(\\w+)\\}"); typedef map ItemToIdMap; } namespace cnoid { class ArchiveSharedData : public Referenced { public: YamlMapping* directoryVariableMap; filesystem::path projectDirPath; filesystem::path topDirPath; filesystem::path shareDirPath; QString topDirString; QString shareDirString; vector idToItems; ItemToIdMap itemToIds; Item* currentParentItem; boost::signal postProcesses; }; bool findSubDirectoryOfDirectoryVariable (ArchiveSharedData* shared, const filesystem::path& path, std::string& out_varName, filesystem::path& out_relativePath) { YamlMapping::const_iterator p; for(p = shared->directoryVariableMap->begin(); p != shared->directoryVariableMap->end(); ++p){ YamlSequence* paths = p->second->toSequence(); if(paths){ for(int i=0; i < paths->size(); ++i){ filesystem::path dirPath(paths[i].toString()); if(findSubDirectory(dirPath, path, out_relativePath)){ out_varName = fromUtf8(p->first); return true; } } } } return false; } void replaceDirectoryVariable(ArchiveSharedData* shared, QString& io_pathString, const QString& varname, int pos, int len) { YamlSequence* paths = shared->directoryVariableMap->findSequence(varname.toStdString()); if(paths){ for(int i=0; i < paths->size(); ++i){ string vpath; QString replaced(io_pathString); replaced.replace(pos, len, paths[i].toString().c_str()); filesystem::file_status fstatus = filesystem::status(filesystem::path(replaced.toStdString())); if(filesystem::is_directory(fstatus) || filesystem::exists(fstatus)) { io_pathString = replaced; return; } } } MessageView::mainInstance()->putln(QString(_("Warning: ${%1} of \"%2\" cannot be expanded !")).arg(varname).arg(io_pathString)); } } ArchivePtr Archive::invalidArchive() { static ArchivePtr invalidArchive_ = new Archive(); invalidArchive_->type_ = YAML_NONE; return invalidArchive_; } Archive::Archive() { } Archive::Archive(int line, int column) : YamlMapping(line, column) { } Archive::~Archive() { } void Archive::initSharedInfo(const std::string& projectFile) { shared = new ArchiveSharedData; shared->directoryVariableMap = AppConfig::archive()->openMapping("PathVariables"); shared->projectDirPath = filesystem::complete(filesystem::path(projectFile)).branch_path(); shared->topDirPath = App::topDirectory(); shared->shareDirPath = App::shareDirectory(); shared->topDirString = shared->topDirPath.string().c_str(); shared->shareDirString = shared->shareDirPath.string().c_str(); shared->currentParentItem = 0; } void Archive::inheritSharedInfoFrom(ArchivePtr archive) { shared = archive->shared; } void Archive::addPostProcess(const boost::function& func) const { if(shared){ shared->postProcesses.connect(func); } } void Archive::callPostProcesses() { if(shared){ shared->postProcesses(); } } Archive* Archive::findSubArchive(const std::string& name) { YamlMapping* mapping = findMapping(name); if(mapping->isValid()){ Archive* archive = dynamic_cast(mapping); if(archive){ return archive; } } return invalidArchive().get(); } bool Archive::readRelocatablePath(const std::string& key, std::string& out_value) const { string orgPathString; if(read(key, orgPathString)){ QString pathString(orgPathString.c_str()); // expand variables in the path int pos = regexVar.indexIn(pathString); if(pos != -1){ int len = regexVar.matchedLength(); if(regexVar.captureCount() > 0){ QString varname = regexVar.cap(1); if(varname == "SHARE"){ pathString.replace(pos, len, shared->shareDirString); } else if(varname == "PROGRAM_TOP"){ pathString.replace(pos, len, shared->topDirString); } else { replaceDirectoryVariable(shared.get(), pathString, varname, pos, len); } } } filesystem::path path(pathString.toStdString()); if(path.is_complete()){ out_value = path.file_string(); } else { filesystem::path fullPath = shared->projectDirPath / path; if(!path.empty() && (*path.begin() == "..")){ filesystem::path compact; makePathCompact(fullPath, compact); out_value = compact.file_string(); } else { out_value = fullPath.file_string(); } } return true; } return false; } /** \todo Use integated nested map whose node is a single path element to be more efficient. */ void Archive::writeRelocatablePath(const std::string& key, const std::string& orgPathString) { filesystem::path orgPath(orgPathString); filesystem::path relativePath; string varName; if(findSubDirectory(shared->projectDirPath, orgPath, relativePath)){ write(key, relativePath.string(), YAML_DOUBLE_QUOTED); } else if(findSubDirectoryOfDirectoryVariable(shared.get(), orgPath, varName, relativePath)){ write(key, string("${") + varName + ("}/") + relativePath.string(), YAML_DOUBLE_QUOTED); } else if(findSubDirectory(shared->shareDirPath, orgPath, relativePath)){ write(key, string("${SHARE}/") + relativePath.string(), YAML_DOUBLE_QUOTED); } else if(findSubDirectory(shared->topDirPath, orgPath, relativePath)){ write(key, string("${PROGRAM_TOP}/") + relativePath.string(), YAML_DOUBLE_QUOTED); } else if(findRelativePath(shared->projectDirPath, orgPath, relativePath)){ write(key, relativePath.string(), YAML_DOUBLE_QUOTED); } } void Archive::registerItemId(Item* item, int id) { if(shared){ if(id >= (signed)shared->idToItems.size()){ shared->idToItems.resize(id + 1); } shared->idToItems[id] = item; shared->itemToIds[item] = id; } } /** @return -1 if item does not belong to the archive */ //int Archive::getItemId(Item* item) const int Archive::getItemId(ItemPtr item) const { if(shared && item){ ItemToIdMap::const_iterator p = shared->itemToIds.find(item); if(p != shared->itemToIds.end()){ return p->second; } } return -1; } void Archive::writeItemId(const std::string& key, ItemPtr item) { if(item){ int id = getItemId(item); if(id >= 0){ write(key, id); } } } Item* Archive::findItem(int id) const { if(shared){ if(id >= 0 && id < (signed)shared->idToItems.size()){ return shared->idToItems[id]; } } return 0; } Item* Archive::currentParentItem() const { if(shared){ return shared->currentParentItem; } return 0; } void Archive::setCurrentParentItem(Item* parentItem) { if(shared){ shared->currentParentItem = parentItem; } } choreonoid-1.1.0+dfsg/src/Base/Archive.h000066400000000000000000000036721207742442300177710ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_GUIBASE_ARCHIVE_H_INCLUDED #define CNOID_GUIBASE_GUIBASE_ARCHIVE_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { class Item; typedef boost::intrusive_ptr ItemPtr; class Archive; typedef boost::intrusive_ptr ArchivePtr; class ArchiveSharedData; class CNOID_EXPORT Archive : public YamlMapping { public: Archive(); Archive(int line, int column); virtual ~Archive(); void initSharedInfo(const std::string& projectFile); void inheritSharedInfoFrom(ArchivePtr archive); void addPostProcess(const boost::function& func) const; Archive* findSubArchive(const std::string& name); //int getItemId(Item* item) const; int getItemId(ItemPtr item) const; Item* findItem(int id) const; template inline ItemType* findItem(int id) const { return dynamic_cast(findItem(id)); } void writeItemId(const std::string& key, ItemPtr item); template inline ItemType* findItem(const std::string& key) const { int id; return read(key, id) ? findItem(id) : 0; } bool readRelocatablePath(const std::string& key, std::string& out_value) const; void writeRelocatablePath(const std::string& key, const std::string& path); Item* currentParentItem() const; private: boost::intrusive_ptr shared; void setCurrentParentItem(Item* parentItem); static ArchivePtr invalidArchive(); void registerItemId(Item* item, int id); // called from ItemTreeArchiver void callPostProcesses(); friend class ItemTreeArchiver; friend class ProjectManagerImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/Base.qrc000066400000000000000000000012361207742442300176120ustar00rootroot00000000000000 icons/setup.png icons/projectsave.png icons/play.png icons/resume.png icons/stop.png icons/refresh.png icons/sceneedit.png icons/viewfitting.png icons/perspective.png icons/floorgrid.png icons/collisionlines.png icons/wireframe.png icons/scenecapture.png icons/graph.png icons/velocitygraph.png icons/accgraph.png choreonoid-1.1.0+dfsg/src/Base/Button.cpp000066400000000000000000000054451207742442300202160ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "Button.h" using namespace cnoid; CheckBox::CheckBox(QWidget* parent) : QCheckBox(parent) { initialize(); } CheckBox::CheckBox(const QString& text, QWidget* parent) : QCheckBox(text, parent) { initialize(); } void CheckBox::initialize() { connect(this, SIGNAL(stateChanged(int)), this, SLOT(onStateChanged(int))); connect(this, SIGNAL(toggled(bool)), this, SLOT(onToggled(bool))); } void CheckBox::onStateChanged(int state) { sigStateChanged_(state); } void CheckBox::onToggled(bool checked) { sigToggled_(checked); } PushButton::PushButton(QWidget* parent) : QPushButton(parent) { initialize(); } PushButton::PushButton(const QString& text, QWidget* parent) : QPushButton(text, parent) { initialize(); } PushButton::PushButton(const QIcon & icon, const QString & text, QWidget* parent) : QPushButton(icon, text, parent) { initialize(); } void PushButton::initialize() { connect(this, SIGNAL(clicked(bool)), this, SLOT(onClicked(bool))); connect(this, SIGNAL(toggled(bool)), this, SLOT(onToggled(bool))); } void PushButton::onClicked(bool checked) { sigClicked_(checked); } void PushButton::onToggled(bool checked) { sigToggled_(checked); } ToggleButton::ToggleButton(QWidget* parent) : PushButton(parent) { setCheckable(true); } ToggleButton::ToggleButton(const QString& text, QWidget* parent) : PushButton(text, parent) { setCheckable(true); } ToggleButton::ToggleButton(const QIcon& icon, const QString& text, QWidget* parent) : PushButton(icon, text, parent) { setCheckable(true); } RadioButton::RadioButton(QWidget* parent) : QRadioButton(parent) { initialize(); } RadioButton::RadioButton(const QString& text, QWidget* parent) : QRadioButton(text, parent) { initialize(); } void RadioButton::initialize() { connect(this, SIGNAL(toggled(bool)), this, SLOT(onToggled(bool))); } void RadioButton::onToggled(bool checked) { sigToggled_(checked); } ToolButton::ToolButton(QWidget* parent) : QToolButton(parent) { initialize(); } /* ToolButton::ToolButton(const QString& text, QWidget* parent) : QToolButton(text, parent) { initialize(); } ToolButton::ToolButton(const QIcon & icon, const QString & text, QWidget* parent) : QToolButton(icon, text, parent) { initialize(); } */ void ToolButton::initialize() { connect(this, SIGNAL(clicked(bool)), this, SLOT(onClicked(bool))); connect(this, SIGNAL(toggled(bool)), this, SLOT(onToggled(bool))); } void ToolButton::onClicked(bool checked) { sigClicked_(checked); } void ToolButton::onToggled(bool checked) { sigToggled_(checked); } ToggleToolButton::ToggleToolButton(QWidget* parent) : ToolButton(parent) { setCheckable(true); } choreonoid-1.1.0+dfsg/src/Base/Button.h000066400000000000000000000064641207742442300176650ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_BUTTON_H_INCLUDED #define CNOID_GUIBASE_BUTTON_H_INCLUDED #include #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT CheckBox : public QCheckBox { Q_OBJECT public: CheckBox(QWidget* parent = 0); CheckBox(const QString& text, QWidget* parent = 0); inline SignalProxy< boost::signal > sigStateChanged() { return sigStateChanged_; } inline SignalProxy< boost::signal > sigToggled() { return sigToggled_; } private Q_SLOTS: void onStateChanged(int state); void onToggled(bool checked); private: boost::signal sigStateChanged_; boost::signal sigToggled_; void initialize(); }; class CNOID_EXPORT PushButton : public QPushButton { Q_OBJECT public: PushButton(QWidget* parent = 0); PushButton(const QString& text, QWidget* parent = 0); PushButton(const QIcon& icon, const QString& text, QWidget* parent = 0); inline SignalProxy< boost::signal > sigClicked() { return sigClicked_; } inline SignalProxy< boost::signal > sigToggled() { return sigToggled_; } private Q_SLOTS: void onClicked(bool checked); void onToggled(bool checked); private: boost::signal sigClicked_; boost::signal sigToggled_; void initialize(); }; class CNOID_EXPORT ToggleButton : public PushButton { public: ToggleButton(QWidget* parent = 0); ToggleButton(const QString& text, QWidget* parent = 0); ToggleButton(const QIcon& icon, const QString& text, QWidget* parent = 0); }; class CNOID_EXPORT RadioButton : public QRadioButton { Q_OBJECT public: RadioButton(QWidget* parent = 0); RadioButton(const QString & text, QWidget* parent = 0); inline SignalProxy< boost::signal > sigToggled() { return sigToggled_; } private Q_SLOTS: void onToggled(bool checked); private: boost::signal sigToggled_; void initialize(); }; class CNOID_EXPORT ToolButton : public QToolButton { Q_OBJECT public: ToolButton(QWidget* parent = 0); inline SignalProxy< boost::signal > sigClicked() { return sigClicked_; } inline SignalProxy< boost::signal > sigToggled() { return sigToggled_; } private Q_SLOTS: void onClicked(bool checked); void onToggled(bool checked); private: boost::signal sigClicked_; boost::signal sigToggled_; void initialize(); }; class CNOID_EXPORT ToggleToolButton : public ToolButton { public: ToggleToolButton(QWidget* parent = 0); //ToggleButton(const QString& text, QWidget* parent = 0); //ToggleButton(const QIcon& icon, const QString& text, QWidget* parent = 0); }; } #endif choreonoid-1.1.0+dfsg/src/Base/ButtonGroup.cpp000066400000000000000000000004731207742442300212270ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ButtonGroup.h" using namespace cnoid; ButtonGroup::ButtonGroup(QObject* parent) : QButtonGroup(parent) { connect(this, SIGNAL(buttonClicked(int)), this, SLOT(onButtonClicked(int))); } void ButtonGroup::onButtonClicked(int id) { sigButtonClicked_(id); } choreonoid-1.1.0+dfsg/src/Base/ButtonGroup.h000066400000000000000000000012041207742442300206650ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_BUTTON_GROUP_H_INCLUDED #define CNOID_GUIBASE_BUTTON_GROUP_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT ButtonGroup : public QButtonGroup { Q_OBJECT public: ButtonGroup(QObject* parent = 0); inline SignalProxy< boost::signal > sigButtonClicked() { return sigButtonClicked_; } private Q_SLOTS: void onButtonClicked(int id); private: boost::signal sigButtonClicked_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/CMakeLists.txt000066400000000000000000000116241207742442300207730ustar00rootroot00000000000000 # @author Shin'ichiro Nakaoka # set(CMAKE_BUILD_TYPE Debug) set(sources App.cpp ItemManager.cpp MenuManager.cpp ProjectManager.cpp ProjectPathSetEditor.cpp PluginManager.cpp MainWindow.cpp ToolBarArea.cpp InfoBar.cpp View.cpp MessageView.cpp SceneView.cpp OsgViewer.cpp ItemTreeView.cpp ItemPropertyView.cpp MultiSeqItemCreationPanel.cpp ToolBar.cpp TimeBar.cpp FileBar.cpp GraphWidget.cpp GraphBar.cpp Action.cpp SpinBox.cpp Slider.cpp ScrollBar.cpp Button.cpp ButtonGroup.cpp LineEdit.cpp ComboBox.cpp Dialog.cpp DescriptionDialog.cpp TreeView.cpp TreeWidget.cpp ItemSelectionModel.cpp ExtensionManager.cpp OptionManager.cpp Plugin.cpp Item.cpp RootItem.cpp FolderItem.cpp SceneItem.cpp MultiSeqItem.cpp MultiValueSeqItem.cpp MultiAffine3SeqItem.cpp Vector3SeqItem.cpp ItemPath.cpp TimeSyncItemEngineManager.cpp AppConfig.cpp Archive.cpp ItemTreeArchiver.cpp SceneObject.cpp SceneGL.cpp ScenePieces.cpp ConnectionSet.cpp LazyCaller.cpp LazySignal.cpp OsgNormalVisualizer.cpp OsgOutlineFx.cpp VrmlToOsgConverter.cpp Licenses.cpp ) set(headers App.h MainWindow.h Action.h SpinBox.h Slider.h ScrollBar.h Button.h ButtonGroup.h LineEdit.h ComboBox.h Dialog.h TreeView.h TreeWidget.h ItemSelectionModel.h MenuManager.h ToolBar.h View.h OsgViewer.h GraphWidget.h ExtensionManager.h OptionManager.h ProjectManager.h PluginManager.h Plugin.h MessageView.h SceneView.h ItemTreeView.h ItemList.h Item.h PutPropertyFunction.h RootItem.h FolderItem.h SceneItem.h MultiSeqItem.h MultiValueSeqItem.h MultiAffine3SeqItem.h Vector3SeqItem.h ItemPath.h TimeBar.h TimeSyncItemEngineManager.h GraphBar.h ItemManager.h AppConfig.h Archive.h ItemTreeArchiver.h SceneObject.h SceneGL.h ScenePieces.h ConnectionSet.h SignalProxy.h LazySignal.h LazyCaller.h VrmlToOsgConverter.h exportdecl.h gettext.h ) QT4_WRAP_CPP(sources MenuManager.h ToolBar.h Action.h SpinBox.h Slider.h ScrollBar.h Button.h ButtonGroup.h ComboBox.h LineEdit.h Dialog.h TreeView.h TreeWidget.h ItemSelectionModel.h InfoBar.h ItemTreeView.h ) QT4_ADD_RESOURCES(RC_SRCS Base.qrc) set(target CnoidBase) make_gettext_mofiles(mofiles) add_library(${target} SHARED ${sources} ${headers} ${mofiles} ${RC_SRCS}) if(APPLE) target_link_libraries(${target} CnoidUtil ${QT_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY} intl osg osgDB osgUtil osgViewer osgGA osgManipulator osgText osgFX osgShadow OpenThreads GL ) elseif(UNIX) #if(CMAKE_BUILD_TYPE STREQUAL "Debug") # set_source_files_properties(VrmlToOsgConverter.cpp PROPERTIES COMPILE_FLAGS "-O3") #endif() target_link_libraries(${target} CnoidUtil ${QT_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY} osg osgDB osgUtil osgViewer osgGA osgManipulator osgText osgFX osgShadow OpenThreads GL icuuc ) elseif(MSVC) target_link_libraries(${target} ${QT_LIBRARIES} optimized osg debug osgd optimized osgDB debug osgDBd optimized osgUtil debug osgUtild optimized osgViewer debug osgViewerd optimized osgGA debug osgGAd optimized osgManipulator debug osgManipulatord optimized osgText debug osgTextd optimized osgFX debug osgFXd optimized osgShadow debug osgShadowd optimized OpenThreads debug OpenThreadsd general opengl32 glu32 CnoidUtil ) endif() apply_common_setting_for_library(${target} "${headers}") install_external_libraries(${QT_BINARY_DIR} ${QT_LIBRARY_DIR} ${QT_LIBRARIES}) install_external_libraries(${Boost_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Boost_PROGRAM_OPTIONS_LIBRARY}) # Installing OpenSceneGraph if(MSVC AND INSTALL_DEPENDENCIES) foreach(library osg osgDB osgUtil osgViewer osgGA osgManipulator osgText osgFX osgShadow OpenThreads) file(GLOB dllfile "${OSG_DIR}/bin/*${library}.dll") install(PROGRAMS ${dllfile} DESTINATION bin CONFIGURATIONS Release) if(INSTALL_SDK) install(FILES ${OSG_DIR}/lib/${library}.lib DESTINATION lib CONFIGURATIONS Release) endif() file(GLOB dllfile "${OSG_DIR}/bin/*${library}d.dll") install(PROGRAMS ${dllfile} DESTINATION bin CONFIGURATIONS Debug) if(INSTALL_SDK) install(FILES ${OSG_DIR}/lib/${library}d.lib DESTINATION lib CONFIGURATIONS Debug) endif() endforeach() file(GLOB pluginpath "${OSG_DIR}/bin/osgPlugins-*") if(pluginpath) file(RELATIVE_PATH pluginfolder ${OSG_DIR} ${pluginpath}) foreach(plugin osgdb_3ds osgdb_dxf osgdb_lwo osgdb_osg osgdb_ply osgdb_pov osgdb_x osgdb_bvh) file(GLOB dllfile "${OSG_DIR}/${pluginfolder}/${plugin}.dll") install(PROGRAMS ${dllfile} DESTINATION ${pluginfolder} CONFIGURATIONS Release) file(GLOB dllfile "${OSG_DIR}/${pluginfolder}/${plugin}d.dll") install(PROGRAMS ${dllfile} DESTINATION ${pluginfolder} CONFIGURATIONS Debug) endforeach() endif() endif() choreonoid-1.1.0+dfsg/src/Base/ComboBox.cpp000066400000000000000000000037531207742442300204530ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "ComboBox.h" #include "gettext.h" using namespace cnoid; ComboBox::ComboBox(QWidget* parent) : QComboBox(parent) { isI18nEnabled = false; connect(this, SIGNAL(activated(int)), this, SLOT(onActivated(int))); connect(this, SIGNAL(currentIndexChanged(int)), this, SLOT(onCurrentIndexChanged(int))); connect(this, SIGNAL(editTextChanged(const QString&)), this, SLOT(onEditTextChanged(const QString&))); connect(this, SIGNAL(highlighted(int)), this, SLOT(onHighlighted(int))); } void ComboBox::enableI18n(const char* domainName) { isI18nEnabled = true; this->domainName = domainName; } void ComboBox::addI18nItem(const char* text) { if(isI18nEnabled){ QComboBox::addItem(dgettext(domainName.c_str(), text), text); } else { QComboBox::addItem(text); } } void ComboBox::addI18nItem(const QIcon & icon, const char* text) { if(isI18nEnabled){ QComboBox::addItem(icon, dgettext(domainName.c_str(), text), text); } else { QComboBox::addItem(icon, text); } } QString ComboBox::currentOrgText() const { if(isI18nEnabled){ return itemData(currentIndex()).toString(); } else { return currentText(); } } int ComboBox::findOrgText(const std::string& orgText, bool setFoundItemCurrent) { QString translatedText; if(isI18nEnabled){ translatedText = dgettext(domainName.c_str(), orgText.c_str()); } else { translatedText = orgText.c_str(); } int index = findText(translatedText); if(index >= 0){ if(setFoundItemCurrent){ setCurrentIndex(index); } } return index; } void ComboBox::onActivated(int index) { sigActivated_(index); } void ComboBox::onCurrentIndexChanged(int index) { sigCurrentIndexChanged_(index); } void ComboBox::onEditTextChanged(const QString& text) { sigEditTextChanged_(text); } void ComboBox::onHighlighted(int index) { sigHighlighted_(index); } choreonoid-1.1.0+dfsg/src/Base/ComboBox.h000066400000000000000000000032321207742442300201100ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_COMBO_BOX_H_INCLUDED #define CNOID_GUIBASE_COMBO_BOX_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT ComboBox : public QComboBox { Q_OBJECT public: ComboBox(QWidget* parent = 0); void enableI18n(const char* domainname); void addI18nItem(const char* text); void addI18nItem(const QIcon & icon, const char* text); QString currentOrgText() const; int findOrgText(const std::string& text, bool setFoundItemCurrent = false); inline SignalProxy< boost::signal > sigActivated() { return sigActivated_; } inline SignalProxy< boost::signal > sigCurrentIndexChanged() { return sigCurrentIndexChanged_; } inline SignalProxy< boost::signal > sigEditTextChanged() { return sigEditTextChanged_; } inline SignalProxy< boost::signal > sigHighlighted() { return sigHighlighted_; } private Q_SLOTS: void onActivated(int index); void onCurrentIndexChanged(int index); void onEditTextChanged(const QString& text); void onHighlighted(int index); private: bool isI18nEnabled; std::string domainName; boost::signal sigActivated_; boost::signal sigCurrentIndexChanged_; boost::signal sigEditTextChanged_; boost::signal sigHighlighted_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ConnectionSet.cpp000066400000000000000000000025771207742442300215210ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ConnectionSet.h" using namespace cnoid; ConnectionSet::ConnectionSet() { } ConnectionSet::ConnectionSet(const ConnectionSet& org) { add(org); } /** This operator disconnects existing connections. */ ConnectionSet& ConnectionSet::operator=(const ConnectionSet& org) { disconnect(); add(org); return *this; } /** Destructor. Note that the connections are *not* disconnected by the destructor. This design is employed to allow a use of the copy constructor and copy operator. */ ConnectionSet::~ConnectionSet() { } void ConnectionSet::disconnect() { for(size_t i=0; i < boostConnections.size(); ++i){ boostConnections[i].disconnect(); } boostConnections.clear(); } void ConnectionSet::add(const boost::signals::connection& connection) { boostConnections.push_back(connection); } void ConnectionSet::add(const ConnectionSet& connections) { for(size_t i=0; i < connections.boostConnections.size(); ++i){ boostConnections.push_back(connections.boostConnections[i]); } } void ConnectionSet::block() { for(size_t i=0; i < boostConnections.size(); ++i){ boostConnections[i].block(); } } void ConnectionSet::unblock() { for(size_t i=0; i < boostConnections.size(); ++i){ boostConnections[i].unblock(); } } choreonoid-1.1.0+dfsg/src/Base/ConnectionSet.h000066400000000000000000000014771207742442300211640ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_CONNECTION_SET_H_INCLUDED #define CNOID_GUIBASE_CONNECTION_SET_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT ConnectionSet { public: ConnectionSet(); ConnectionSet(const ConnectionSet& org); ConnectionSet& operator=(const ConnectionSet& org); ~ConnectionSet(); inline bool empty() { return boostConnections.empty(); } void add(const boost::signals::connection& connection); void add(const ConnectionSet& connections); void block(); void unblock(); void disconnect(); private: std::vector boostConnections; }; } #endif choreonoid-1.1.0+dfsg/src/Base/DescriptionDialog.cpp000066400000000000000000000020671207742442300223430ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "DescriptionDialog.h" #include "MainWindow.h" #include "Separator.h" #include #include #include #include #include "gettext.h" using namespace cnoid; DescriptionDialog::DescriptionDialog() : Dialog(MainWindow::instance()) { QVBoxLayout* vbox = new QVBoxLayout(); textEdit.setFrameShape(QFrame::NoFrame); textEdit.setReadOnly(true); vbox->addWidget(&textEdit); vbox->addWidget(new HSeparator()); QPushButton* okButton = new QPushButton(_("&Ok")); okButton->setDefault(true); QDialogButtonBox* buttonBox = new QDialogButtonBox(this); buttonBox->addButton(okButton, QDialogButtonBox::AcceptRole); connect(buttonBox,SIGNAL(accepted()), this, SLOT(accept())); vbox->addWidget(buttonBox); setLayout(vbox); QFontMetrics metrics(font()); resize(metrics.averageCharWidth() * 70, metrics.height() * 24); } void DescriptionDialog::setDescription(const QString& text) { textEdit.setPlainText(text); } choreonoid-1.1.0+dfsg/src/Base/DescriptionDialog.h000066400000000000000000000007221207742442300220040ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_DESCRIPTION_DIALOG_H_INCLUDED #define CNOID_GUIBASE_DESCRIPTION_DIALOG_H_INCLUDED #include "Dialog.h" #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT DescriptionDialog : public Dialog { public: DescriptionDialog(); void setDescription(const QString& text); private: QPlainTextEdit textEdit; }; } #endif choreonoid-1.1.0+dfsg/src/Base/Dialog.cpp000066400000000000000000000011101207742442300201230ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "Dialog.h" using namespace cnoid; Dialog::Dialog(QWidget* parent, Qt::WindowFlags f) : QDialog(parent, f) { initialize(); } void Dialog::initialize() { connect(this, SIGNAL(accepted()), this, SLOT(onAccepted())); connect(this, SIGNAL(finished(int)), this, SLOT(onFinished(int))); connect(this, SIGNAL(rejected()), this, SLOT(onRejected())); } void Dialog::onAccepted() { sigAccepted_(); } void Dialog::onFinished(int result) { sigFinished_(result); } void Dialog::onRejected() { sigRejected_(); } choreonoid-1.1.0+dfsg/src/Base/Dialog.h000066400000000000000000000017651207742442300176100ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_DIALOG_H_INCLUDED #define CNOID_GUIBASE_DIALOG_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT Dialog : public QDialog { Q_OBJECT public: Dialog(QWidget* parent = 0, Qt::WindowFlags f = 0); inline SignalProxy< boost::signal > sigAccepted() { return sigAccepted_; } inline SignalProxy< boost::signal > sigFinished() { return sigFinished_; } inline SignalProxy< boost::signal > sigRejected() { return sigRejected_; } private Q_SLOTS: void onAccepted(); void onFinished(int result); void onRejected(); private: boost::signal sigAccepted_; boost::signal sigFinished_; boost::signal sigRejected_; void initialize(); }; } #endif choreonoid-1.1.0+dfsg/src/Base/ExtensionManager.cpp000066400000000000000000000125521207742442300222070ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "ExtensionManager.h" #include "ItemManager.h" #include "MenuManager.h" #include "OptionManager.h" #include "ProjectManager.h" #include "Plugin.h" #include "Item.h" #include "View.h" #include "ToolBar.h" #include "MainWindow.h" #include "App.h" #include "TimeSyncItemEngineManager.h" #include "LazyCaller.h" #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class ExtensionManagerImpl { public: ExtensionManagerImpl(ExtensionManager* self, const std::string& moduleName); ~ExtensionManagerImpl(); void setVersion(const std::string& version, bool isPlugin); void deleteManagedObjects();; ExtensionManager* self; scoped_ptr menuManager; scoped_ptr itemManager; scoped_ptr timeSyncItemEngineManger; string moduleName; string textDomain; stack pointerHolders; boost::signal sigSystemUpdated; boost::signal sigReleaseRequest; }; } namespace { set extensionManagerImpls; void emitSigSystemUpdated() { set::iterator p = extensionManagerImpls.begin(); while(p != extensionManagerImpls.end()){ (*p)->sigSystemUpdated(); ++p; } } LazyCaller emitSigSystemUpdatedCaller(emitSigSystemUpdated); } ExtensionManager::ExtensionManager(const std::string& moduleName, bool isPlugin) { impl = new ExtensionManagerImpl(this, moduleName); impl->setVersion(CNOID_FULL_VERSION_STRING, isPlugin); } ExtensionManager::ExtensionManager(const std::string& moduleName, const std::string& version, bool isPlugin) { impl = new ExtensionManagerImpl(this, moduleName); impl->setVersion(version, isPlugin); } ExtensionManagerImpl::ExtensionManagerImpl(ExtensionManager* self, const std::string& moduleName) : self(self), moduleName(moduleName) { extensionManagerImpls.insert(this); } ExtensionManager::~ExtensionManager() { delete impl; } ExtensionManagerImpl::~ExtensionManagerImpl() { ProjectManager::instance()->disconnectArchivers(moduleName); deleteManagedObjects(); sigReleaseRequest(); extensionManagerImpls.erase(this); } ItemManager& ExtensionManager::itemManager() { if(!impl->itemManager){ impl->itemManager.reset(new ItemManager(impl->moduleName, menuManager())); impl->itemManager->bindTextDomain(impl->textDomain); } return *impl->itemManager; } TimeSyncItemEngineManager& ExtensionManager::timeSyncItemEngineManger() { if(!impl->timeSyncItemEngineManger){ impl->timeSyncItemEngineManger.reset(new TimeSyncItemEngineManager(impl->moduleName)); } return *impl->timeSyncItemEngineManger; } MenuManager& ExtensionManager::menuManager() { if(!impl->menuManager){ impl->menuManager.reset(new MenuManager(MainWindow::instance()->menuBar())); impl->menuManager->bindTextDomain(impl->textDomain); } return *impl->menuManager; } OptionManager& ExtensionManager::optionManager() { static OptionManager optionManager; return optionManager; } void ExtensionManagerImpl::setVersion(const std::string& version, bool isPlugin) { vector v; boost::algorithm::split(v, version, boost::is_any_of(".")); textDomain = string("Cnoid") + moduleName; if(isPlugin){ textDomain += "Plugin"; } if(!v.empty()){ textDomain += "-"; textDomain += v[0]; if(v.size() >= 2){ textDomain += string(".") + v[1]; } } bindtextdomain(textDomain.c_str(), (App::topDirectory() + "/" + CNOID_LOCALE_SUBDIR).c_str()); } const char* ExtensionManager::textDomain() const { return impl->textDomain.c_str(); } ExtensionManager::PtrHolderBase::~PtrHolderBase() { } void ExtensionManager::manageSub(PtrHolderBase* holder) { impl->pointerHolders.push(holder); } void ExtensionManager::addView(View* view) { manage(view); if(!impl->textDomain.empty()){ view->setWindowTitle(dgettext(impl->textDomain.c_str(), view->objectName().toAscii())); } MainWindow::instance()->addView(impl->moduleName, view); } void ExtensionManager::addToolBar(ToolBar* toolBar) { manage(toolBar); MainWindow::instance()->addToolBar(toolBar); } void ExtensionManagerImpl::deleteManagedObjects() { while(!pointerHolders.empty()){ ExtensionManager::PtrHolderBase* holder = pointerHolders.top(); delete holder; pointerHolders.pop(); } } SignalProxy< boost::signal > ExtensionManager::sigSystemUpdated() { return impl->sigSystemUpdated; } void ExtensionManager::notifySystemUpdate() { emitSigSystemUpdatedCaller.request(); } SignalProxy< boost::signal > ExtensionManager::sigReleaseRequest() { return impl->sigReleaseRequest; } void ExtensionManager::connectProjectArchiver( const std::string& name, boost::function storeFunction, boost::function restoreFunction) { ProjectManager::instance()->connectArchiver(impl->moduleName, name, storeFunction, restoreFunction); } choreonoid-1.1.0+dfsg/src/Base/ExtensionManager.h000066400000000000000000000064271207742442300216600ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_EXTENSION_MANAGER_H_INCLUDED #define CNOID_GUIBASE_EXTENSION_MANAGER_H_INCLUDED #include "SignalProxy.h" #include #include #include "exportdecl.h" namespace cnoid { class Item; class View; class ToolBar; class Archive; class ItemManager; class TimeSyncItemEngineManager; class MenuManager; class OptionManager; class ExtensionManagerImpl; class CNOID_EXPORT ExtensionManager { public: ExtensionManager(const std::string& moduleName, bool isPlugin); ExtensionManager(const std::string& moduleName, const std::string& version, bool isPlugin); virtual ~ExtensionManager(); ItemManager& itemManager(); TimeSyncItemEngineManager& timeSyncItemEngineManger(); MenuManager& menuManager(); OptionManager& optionManager(); const char* textDomain() const; private: struct CNOID_EXPORT PtrHolderBase { virtual ~PtrHolderBase(); }; // smart pointer version template struct PtrHolder : public PtrHolderBase { PtrHolder(PointerType pointer) : pointer(pointer) { } virtual ~PtrHolder() { } PointerType pointer; }; // raw pointer version template struct PtrHolder : public PtrHolderBase { PtrHolder(Object* pointer) : pointer(pointer) { } virtual ~PtrHolder() { delete pointer; } Object* pointer; }; void manageSub(PtrHolderBase* holder); public: void addView(View* view); void addToolBar(ToolBar* toolBar); template PointerType manage(PointerType pointer) { manageSub(new PtrHolder(pointer)); return pointer; } /** @if jp 起動時ã«ã‚ªãƒ–ジェクトã®åˆæœŸç”ŸæˆãŒå…¨ã¦çµ‚ã‚ã£ãŸæ™‚や〠新ãŸã«ãƒ—ラグインã®èª­ã¿è¾¼ã¿ãƒ»è§£æ”¾ãŒã‚ã£ãŸã¨ããªã©ã€ システムã§åˆ©ç”¨å¯èƒ½ãªã‚ªãƒ–ジェクトã®çŠ¶æ…‹ã«æ›´æ–°ãŒã‚ã£ãŸã¨ã㫠発行ã•ã‚Œã‚‹ 'SystemUpdated' シグナルã¨æŽ¥ç¶šã™ã‚‹ã€‚ @endif */ SignalProxy< boost::signal > sigSystemUpdated(); /** @if jp 'SystemUpdated' シグナルを発行ã™ã‚‹ã€‚ シグナルã®ç™ºè¡Œã¯æœ¬ãƒ¡ã‚½ãƒƒãƒ‰å‘¼ã³å‡ºã—時ã§ã¯ãªãã€å¾Œã»ã©ã‚¤ãƒ™ãƒ³ãƒˆãƒ«ãƒ¼ãƒ—内ã«ã¦è¡Œã‚れる。 ã“ã‚Œã«ã‚ˆã‚Šã€ã‚¹ãƒ­ãƒƒãƒˆã®å‡¦ç†ã¯ç¾åœ¨å‡¦ç†ä¸­ã®ä»–ã®ã‚¤ãƒ™ãƒ³ãƒˆãŒå‡¦ç†ã•ã‚ŒãŸå¾Œã«ãªã‚‹ã€‚ ã¾ãŸã€æœ¬ãƒ¡ã‚½ãƒƒãƒ‰ãŒåŒæ™‚期ã«è¤‡æ•°å›žå‘¼ã°ã‚ŒãŸå ´åˆã§ã‚‚ã€ã‚·ã‚°ãƒŠãƒ«ã®ç™ºè¡Œã¯ã²ã¨ã¤ã«ã¾ã¨ã‚られる。 @endif */ static void notifySystemUpdate(); SignalProxy< boost::signal > sigReleaseRequest(); void connectProjectArchiver( const std::string& name, boost::function storeFunction, boost::function restoreFunction); private: ExtensionManager(const ExtensionManager& org); // disable the copy constructor ExtensionManagerImpl* impl; friend class ExtensionManagerImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/FileBar.cpp000066400000000000000000000014331207742442300202400ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "FileBar.h" #include "ExtensionManager.h" #include "ProjectManager.h" #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; void FileBar::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ext->addToolBar(instance()); initialized = true; } } FileBar* FileBar::instance() { static FileBar* fileBar = new FileBar(); return fileBar; } FileBar::FileBar() : ToolBar(N_("FileBar")) { addButton(QIcon(":/Base/icons/projectsave.png"), _("Save the project")) ->sigClicked().connect( bind(&ProjectManager::overwriteCurrentProject, ProjectManager::instance())); } FileBar::~FileBar() { } choreonoid-1.1.0+dfsg/src/Base/FileBar.h000066400000000000000000000006341207742442300177070ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_FILE_BAR_H_INCLUDED #define CNOID_GUIBASE_FILE_BAR_H_INCLUDED #include namespace cnoid { class FileBar : public ToolBar { public: static void initialize(ExtensionManager* ext); static FileBar* instance(); virtual ~FileBar(); private: FileBar(); }; } #endif choreonoid-1.1.0+dfsg/src/Base/FolderItem.cpp000066400000000000000000000015241207742442300207670ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "FolderItem.h" #include "ItemManager.h" #include "gettext.h" using namespace cnoid; FolderItem::FolderItem() { } FolderItem::FolderItem(const std::string& name) : Item(name) { } FolderItem::FolderItem(const FolderItem& org) : Item(org) { } FolderItem::~FolderItem() { } ItemPtr FolderItem::doDuplicate() const { return new FolderItem(*this); } bool FolderItem::store(Archive& archive) { return true; } bool FolderItem::restore(const Archive& archive) { return true; } void FolderItem::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ItemManager& im = ext->itemManager(); im.registerClass(N_("FolderItem")); im.addCreationPanel(); initialized = true; } } choreonoid-1.1.0+dfsg/src/Base/FolderItem.h000066400000000000000000000013041207742442300204300ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_FOLDER_ITEM_H_INCLUDED #define CNOID_GUIBASE_FOLDER_ITEM_H_INCLUDED #include "Item.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT FolderItem : public Item { public: static void initialize(ExtensionManager* ext); FolderItem(); FolderItem(const std::string& name); FolderItem(const FolderItem& org); protected: virtual ~FolderItem(); virtual ItemPtr doDuplicate() const; virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); }; typedef boost::intrusive_ptr FolderItemPtr; } #endif choreonoid-1.1.0+dfsg/src/Base/GraphBar.cpp000066400000000000000000000314111207742442300204210ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "GraphBar.h" #include "GraphWidget.h" #include "ConnectionSet.h" #include "ExtensionManager.h" #include "SpinBox.h" #include "ComboBox.h" #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class GraphBarImpl { public: GraphBarImpl(GraphBar* self); GraphBar* self; ConnectionSet connections; ToolButton* orgRenderingToggle; ToolButton* velRenderingToggle; ToolButton* accRenderingToggle; ToolButton* optionToggle; vector optWidgets; DoubleSpinBox verticalValueRangeSpin; SpinBox lineWidthSpin; ToolButton* limitValueToggle; ToolButton* gridToggle; DoubleSpinBox* gridSizeSpin; ToolButton* rulersToggle; ToolButton* timeBarSyncToggle; ComboBox* autoScrollModeCombo; ToolButton* editModeToggle; ToolButton* freeLineModeRadio; ToolButton* lineModeRadio; SpinBox* controlPointStepSpin; SpinBox* controlPointOffsetSpin; ToolButton* highlightingControlPointsToggle; GraphWidget* focusedGraphWidget; void onOptionToggled(); void focus(GraphWidget* graphWidget, bool forceUpdate); void onVerticalValueRangeChanged(double value); void onLineWidthChanged(int value); void onRenderingTypesToggled(); void onLimitValueToggled(bool on); void onGridToggled(bool on); void onGridSizeChanged(double size); void onRulersToggled(bool on); void onTimeBarSyncToggled(bool on); void onAutoScrollModeChanged(int index); void onEditModeToggled(bool on); void onFreeLineModeToggled(bool on); void onLineModeToggled(bool on); void onControlPointStepOrOffsetChanged(); void onHighlightingControlPointsToggled(bool on); }; } void GraphBar::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ext->addToolBar(GraphBar::instance()); initialized = true; } } GraphBar* GraphBar::instance() { static GraphBar* graphBar = new GraphBar(); return graphBar; } GraphBar::GraphBar() : ToolBar(N_("GraphBar")) { impl = new GraphBarImpl(this); } GraphBarImpl::GraphBarImpl(GraphBar* self) : self(self) { orgRenderingToggle = self->addToggleButton(QIcon(":/Base/icons/graph.png"), _("Plot trajectories of the target data on the graph view")); orgRenderingToggle->setChecked(true); connections.add( orgRenderingToggle->sigToggled().connect( bind(&GraphBarImpl::onRenderingTypesToggled, this))); velRenderingToggle = self->addToggleButton(QIcon(":/Base/icons/velocitygraph.png"), _("Plot velocity trajectories")); connections.add( velRenderingToggle->sigToggled().connect( bind(&GraphBarImpl::onRenderingTypesToggled, this))); accRenderingToggle = self->addToggleButton(QIcon(":/Base/icons/accgraph.png"), _("Plot acceleration trajectories")); connections.add( accRenderingToggle->sigToggled().connect( bind(&GraphBarImpl::onRenderingTypesToggled, this))); optionToggle = self->addToggleButton(QIcon(":/Base/icons/setup.png"), _("Show options of the graph view")); connections.add( optionToggle->sigToggled().connect( bind(&GraphBarImpl::onOptionToggled, this))); optWidgets.push_back(self->addLabel("y-range, 1.0e")); verticalValueRangeSpin.setDecimals(1); verticalValueRangeSpin.setRange(-99.8, 99.8); verticalValueRangeSpin.setSingleStep(0.1); verticalValueRangeSpin.setValue(1.0); connections.add( verticalValueRangeSpin.sigValueChanged().connect( bind(&GraphBarImpl::onVerticalValueRangeChanged, this, _1))); self->addWidget(&verticalValueRangeSpin); optWidgets.push_back(&verticalValueRangeSpin); limitValueToggle = self->addToggleButton( _("Limit"), _("Show the lower / upper limit values on the graph. Trajectory edit is also limited within the values.")); limitValueToggle->setChecked(true); connections.add( limitValueToggle->sigToggled().connect( bind(&GraphBarImpl::onLimitValueToggled, this, _1))); optWidgets.push_back(limitValueToggle); optWidgets.push_back(self->addLabel(_("LW"))); lineWidthSpin.setRange(1, 9); lineWidthSpin.setValue(1); connections.add( lineWidthSpin.sigValueChanged().connect( bind(&GraphBarImpl::onLineWidthChanged, this, _1))); self->addWidget(&lineWidthSpin); optWidgets.push_back(&lineWidthSpin); gridToggle = self->addToggleButton(_("Grid"), _("Show a grid on the graph background")); connections.add( gridToggle->sigToggled().connect( bind(&GraphBarImpl::onGridToggled, this, _1))); optWidgets.push_back(gridToggle); gridSizeSpin = new DoubleSpinBox(); gridSizeSpin->setDecimals(3); gridSizeSpin->setRange(-999.999, 999.999); gridSizeSpin->setSingleStep(0.001); gridSizeSpin->setValue(0.2); connections.add( gridSizeSpin->sigValueChanged().connect( bind(&GraphBarImpl::onGridSizeChanged, this, _1))); self->addWidget(gridSizeSpin); optWidgets.push_back(gridSizeSpin); rulersToggle = self->addToggleButton(_("Ruler"), _("Show rulers")); rulersToggle->setChecked(true); connections.add( rulersToggle->sigToggled().connect( bind(&GraphBarImpl::onRulersToggled, this, _1))); optWidgets.push_back(rulersToggle); optWidgets.push_back(self->addSeparator()); editModeToggle = self->addToggleButton(_("Edit"), _("Edit mode on/off")); connections.add( editModeToggle->sigToggled().connect( bind(&GraphBarImpl::onEditModeToggled, this, _1))); optWidgets.push_back(editModeToggle); freeLineModeRadio = self->addRadioButton(_("Free"), _("Free line edit mode")); freeLineModeRadio->setChecked(true); connections.add( freeLineModeRadio->sigToggled().connect( bind(&GraphBarImpl::onFreeLineModeToggled, this, _1))); optWidgets.push_back(freeLineModeRadio); lineModeRadio = self->addRadioButton(_("Line"), _("Line edit mode")); connections.add( lineModeRadio->sigToggled().connect( bind(&GraphBarImpl::onLineModeToggled, this, _1))); optWidgets.push_back(lineModeRadio); optWidgets.push_back(self->addLabel(_("step"))); controlPointStepSpin = new SpinBox(); controlPointStepSpin->setRange(1, 999); controlPointStepSpin->setValue(1); connections.add( controlPointStepSpin->sigValueChanged().connect( bind(&GraphBarImpl::onControlPointStepOrOffsetChanged, this))); self->addWidget(controlPointStepSpin); optWidgets.push_back(controlPointStepSpin); optWidgets.push_back(self->addLabel(_("offset"))); controlPointOffsetSpin = new SpinBox(); controlPointOffsetSpin->setRange(0, 999); controlPointOffsetSpin->setValue(0); connections.add( controlPointOffsetSpin->sigValueChanged().connect( bind(&GraphBarImpl::onControlPointStepOrOffsetChanged, this))); self->addWidget(controlPointOffsetSpin); optWidgets.push_back(controlPointOffsetSpin); highlightingControlPointsToggle = self->addToggleButton(_("CP"), _("Highlighting control points")); connections.add( highlightingControlPointsToggle->sigToggled().connect( bind(&GraphBarImpl::onHighlightingControlPointsToggled, this, _1))); optWidgets.push_back(highlightingControlPointsToggle); optWidgets.push_back(self->addSeparator()); timeBarSyncToggle = self->addToggleButton(_("Sync"), _("Cursor movement on a graph updates the time bar position")); timeBarSyncToggle->setChecked(true); connections.add( timeBarSyncToggle->sigToggled().connect( bind(&GraphBarImpl::onTimeBarSyncToggled, this, _1))); optWidgets.push_back(timeBarSyncToggle); //self->addLabel("Auto Scroll"); autoScrollModeCombo = new ComboBox(self); autoScrollModeCombo->addItem(_("off")); autoScrollModeCombo->addItem(_("cont.")); autoScrollModeCombo->addItem(_("page")); autoScrollModeCombo->setCurrentIndex(1); connections.add( autoScrollModeCombo->sigCurrentIndexChanged().connect( bind(&GraphBarImpl::onAutoScrollModeChanged, this, _1))); self->addWidget(autoScrollModeCombo); optWidgets.push_back(autoScrollModeCombo); self->setEnabled(false); onOptionToggled(); focusedGraphWidget = 0; } GraphBar::~GraphBar() { delete impl; } void GraphBarImpl::onOptionToggled() { if(optionToggle->isChecked()){ for(size_t i=0; i < optWidgets.size(); ++i){ optWidgets[i]->show(); } } else { for(size_t i=0; i < optWidgets.size(); ++i){ optWidgets[i]->hide(); } } } GraphWidget* GraphBar::focusedGraphWidget() { return impl->focusedGraphWidget; } void GraphBar::focus(GraphWidget* graph, bool forceUpdate) { impl->focus(graph, forceUpdate); } void GraphBarImpl::focus(GraphWidget* graph, bool forceUpdate) { if(graph && (forceUpdate || (graph != focusedGraphWidget))){ focusedGraphWidget = graph; self->setEnabled(true); connections.block(); editModeToggle->setChecked(graph->mode() == GraphWidget::EDIT_MODE); GraphWidget::EditMode editMode = graph->editMode(); if(editMode == GraphWidget::FREE_LINE_MODE){ freeLineModeRadio->setChecked(true); } else if(editMode == GraphWidget::LINE_MODE){ lineModeRadio->setChecked(true); } bool org, vel, acc; graph->getRenderingTypes(org, vel, acc); orgRenderingToggle->setChecked(org); velRenderingToggle->setChecked(vel); accRenderingToggle->setChecked(acc); double lower, upper; graph->getVerticalValueRange(lower, upper); verticalValueRangeSpin.setValue(upper); lineWidthSpin.setValue(graph->getLineWidth()); timeBarSyncToggle->setChecked(graph->isTimeBarSyncMode()); autoScrollModeCombo->setCurrentIndex(graph->autoScrollMode()); limitValueToggle->setChecked(graph->showsLimits()); rulersToggle->setChecked(graph->showsRulers()); gridToggle->setChecked(graph->showsGrid()); double width, height; graph->getGridSize(width, height); gridSizeSpin->setValue(width); int step, offset; graph->getControlPointStep(step, offset); controlPointStepSpin->setValue(step); controlPointOffsetSpin->setValue(offset); highlightingControlPointsToggle->setChecked(graph->highlightsControlPoints()); connections.unblock(); } } void GraphBar::releaseFocus(GraphWidget* graphWidget) { if(impl->focusedGraphWidget == graphWidget){ impl->focusedGraphWidget = 0; setEnabled(false); } } void GraphBarImpl::onVerticalValueRangeChanged(double value) { double upper = pow(10.0, value); double lower = -upper; focusedGraphWidget->setVerticalValueRange(lower, upper); } void GraphBarImpl::onLineWidthChanged(int value) { focusedGraphWidget->setLineWidth(value); } void GraphBarImpl::onRenderingTypesToggled() { focusedGraphWidget->setRenderingTypes (orgRenderingToggle->isChecked(), velRenderingToggle->isChecked(), accRenderingToggle->isChecked()); } void GraphBarImpl::onLimitValueToggled(bool on) { focusedGraphWidget->showLimits(on); } void GraphBarImpl::onGridToggled(bool on) { focusedGraphWidget->showGrid(on); } void GraphBarImpl::onGridSizeChanged(double size) { focusedGraphWidget->setGridSize(size, size); } void GraphBarImpl::onRulersToggled(bool on) { focusedGraphWidget->showRulers(on); } void GraphBarImpl::onTimeBarSyncToggled(bool on) { focusedGraphWidget->setTimeBarSyncMode(on); } void GraphBarImpl::onAutoScrollModeChanged(int index) { focusedGraphWidget->setAutoScrollMode((GraphWidget::ScrollMode)index); } void GraphBarImpl::onEditModeToggled(bool on) { GraphWidget::Mode mode = on ? GraphWidget::EDIT_MODE : GraphWidget::VIEW_MODE; focusedGraphWidget->changeMode(mode); } void GraphBarImpl::onFreeLineModeToggled(bool on) { if(on){ focusedGraphWidget->changeEditMode(GraphWidget::FREE_LINE_MODE); } } void GraphBarImpl::onLineModeToggled(bool on) { if(on){ focusedGraphWidget->changeEditMode(GraphWidget::LINE_MODE); } } void GraphBarImpl::onControlPointStepOrOffsetChanged() { focusedGraphWidget->setControlPointStep( controlPointStepSpin->value(), controlPointOffsetSpin->value()); } void GraphBarImpl::onHighlightingControlPointsToggled(bool on) { focusedGraphWidget->highlightControlPoints(on); } choreonoid-1.1.0+dfsg/src/Base/GraphBar.h000066400000000000000000000014251207742442300200700ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_GRAPH_BAR_H_INCLUDED #define CNOID_GUIBASE_GRAPH_BAR_H_INCLUDED #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class GraphWidget; class GraphBarImpl; class CNOID_EXPORT GraphBar : public ToolBar { public: static void initialize(ExtensionManager* ext); static GraphBar* instance(); GraphWidget* focusedGraphWidget(); void focus(GraphWidget* graphWidget, bool forceUpdate = false); void releaseFocus(GraphWidget* graphWidget); private: GraphBar(); virtual ~GraphBar(); GraphBarImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/GraphWidget.cpp000066400000000000000000001517101207742442300211450ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "GraphWidget.h" #include "GraphBar.h" #include "ToolBar.h" #include "TimeBar.h" #include "ToolBarArea.h" #include "ScrollBar.h" #include "ConnectionSet.h" #include #include #include #include #include #include #include #include #include #include #include #include #ifdef _MSC_VER #include inline int isfinite(double x) { return _finite(x); } #endif #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; const size_t MAX_NUM_HISTORIES = 10; struct EditHistory { EditHistory() { frame = 0; } int frame; vector orgValues; vector newValues; }; typedef shared_ptr EditHistoryPtr; typedef deque EditHistoryList; } namespace cnoid { class GraphDataHandlerImpl { public: GraphDataHandlerImpl(); boost::signal sigDataUpdated; /** The size of this array is the actual data size + 2 and the actual data is stored from the second elements. The prepended / appended elements are same as the initial / last elements of the actual data, and the elements are used for calculating velocities. */ vector values; int numFrames; // the actual number of frames (values.size() - 2) int prevNumValues; double offset; double stepRatio; double lowerValueLimit; double upperValueLimit; double lowerVelocityLimit; double upperVelocityLimit; std::string label; float r, g, b; EditHistoryList editHistories; int currentHistory; dynamic_bitset<> controlPointMask; bool isControlPointUpdateNeeded; GraphDataHandler::DataRequestCallback dataRequestCallback; GraphDataHandler::DataModifiedCallback dataModifiedCallback; }; class GraphWidgetImpl : public signals::trackable { public: GraphWidget* self; View* parentView; QWidget* screen; DoubleScrollBar* hScrollbar; DoubleScrollBar* vScrollbar; QPen pen; QVector cursorDashes; QVector limitValueDashes; QPolygonF polyline; GraphWidget::Mode mode; GraphWidget::EditMode editMode; std::vector handlers; ConnectionSet connections; bool isOrgValueVisible; bool isVelocityVisible; bool isAccelerationVisible; bool isReconfigurationNeeded; bool isDomainUpdateNeeded; double domainLowerX; double domainUpperX; double rangeLowerY; double rangeUpperY; double cursorX; double screenCursorX; double leftX; double centerY; double scaleX; double scaleY; double visibleWidth; double visibleHeight; double visibleHeightHalf; double bottomY; double topY; double screenMarginX; double screenCenterY; double screenWidth; double screenHeight; double lineWidth; bool isLimitVisible; bool isGridOn; double gridWidth; double gridHeight; GraphDataHandlerImpl* editTarget; bool isControlPointsHighlighted; int controlPointStep; int controlPointOffset; enum { DRAG_NONE, DRAG_CURSOR, DRAG_EDIT, DRAG_ZOOM, DRAG_MOVE } dragState; double pressedScreenX; double pressedScreenY; double dragPrevScreenX; double dragPrevScreenY; double dragOrgScaleX; double dragOrgScaleY; double dragOrgLeftX; double dragOrgCenterY; bool isEditBufferedUpdateMode; int editedFrameBegin; int editedFrameEnd; GraphWidget::ScrollMode autoScrollMode; TimeBar* timeBar; bool timeBarSyncMode; boost::signals::connection timeChangedConnetion; QLabel statusLabel; GraphWidgetImpl(GraphWidget* self, View* parentView); ~GraphWidgetImpl(); void onActivated(); void onDeactivated(); void addDataHandler(GraphDataHandlerPtr handler); void clearDataHandlers(); void updateData(GraphDataHandlerPtr handler); bool setCursorPosition(double x, bool enableTimeBarSync, bool forceTimeChange); bool setCursorPositionWithScreenX(double screenX, bool enableTimeBarSync, bool forceTimeChange); void setTimeBarSyncMode(bool on); void enableTimeBarSync(bool on); void setAutoScrollMode(GraphWidget::ScrollMode mode); void setVerticalValueRange(double lower, double upper); void showLimits(bool show); void showRulers(bool show); void showGrid(bool show); void setGridSize(double width, double height); void setControlPointStep(int step, int offset); void highlightControlPoints(bool on); void changeMode(GraphWidget::Mode mode); void changeEditMode(GraphWidget::EditMode mode); void setupConfiguration(); void updateDomain(); bool onFocusInEvent(QFocusEvent* event); bool onScreenResizeEvent(QResizeEvent* event); bool onScreenMouseButtonPressEvent(QMouseEvent* event); bool onScreenMouseButtonReleaseEvent(QMouseEvent* event); bool onScreenMouseMoveEvent(QMouseEvent* event); bool onScreenKeyPressEvent(QKeyEvent* event); bool onScreenKeyReleaseEvent(QKeyEvent* event); void zoomScreen(int screenX, int screenY); void moveScreen(double screenX, double screenY); void startTrajectoryEdit(); void finishTrajectoryEdit(); void doTrajectoryEdit(int screenX, int screenY, Qt::KeyboardModifiers modifiers); void storeOrgValuesToHistory(int frameBegin, int frameEnd); void undoTrajectoryEdit(); void redoTrajectoryEdit(); void selectEditTargetByClicking(double screenX, double screenY); bool onScreenPaintEvent(QPaintEvent* event); void drawTrajectory(QPainter& painter, const QRect& rect, GraphDataHandlerImpl* data); void drawLimits(QPainter& painter, GraphDataHandlerImpl* data); void updateControlPoints(GraphDataHandlerImpl* data); void drawGrid(QPainter& painter); void onHScrollbarChanged(double value); void onVScrollbarChanged(double value); bool storeState(Archive& archive); bool restoreState(const Archive& archive); }; } GraphDataHandler::GraphDataHandler() { impl = new GraphDataHandlerImpl(); } GraphDataHandlerImpl::GraphDataHandlerImpl() { offset = 0.0; stepRatio = 1.0; r = 0.0f; g = 0.0f; b = 0.8f; lowerValueLimit = -std::numeric_limits::max(); upperValueLimit = std::numeric_limits::max(); lowerVelocityLimit = -std::numeric_limits::max(); upperVelocityLimit = std::numeric_limits::max(); isControlPointUpdateNeeded = true; currentHistory = 0; } GraphDataHandler::~GraphDataHandler() { delete impl; } void GraphDataHandler::setColor(float r, float g, float b) { impl->r = r; impl->g = g; impl->b = b; } void GraphDataHandler::setLabel(const std::string& label) { impl->label = label; } void GraphDataHandler::setFrameProperties(int numFrames, double frameRate, double offset) { impl->values.resize(numFrames + 2); impl->numFrames = numFrames; impl->stepRatio = 1.0 / frameRate; impl->offset = offset; impl->isControlPointUpdateNeeded = true; } void GraphDataHandler::setValueLimits(double lower, double upper) { impl->lowerValueLimit = lower; impl->upperValueLimit = upper; } void GraphDataHandler::setVelocityLimits(double lower, double upper) { impl->lowerVelocityLimit = lower; impl->upperVelocityLimit = upper; } void GraphDataHandler::addVerticalLine(double x, const std::string& label) { } void GraphDataHandler::addHorizontalLine(double y, const std::string& label) { } void GraphDataHandler::clearLines() { } void GraphDataHandler::update() { if(TRACE_FUNCTIONS){ cout << "GraphDataHandler::update()" << endl; } impl->sigDataUpdated(); } void GraphDataHandler::setDataRequestCallback(DataRequestCallback callback) { impl->dataRequestCallback = callback; } void GraphDataHandler::setDataModifiedCallback(DataModifiedCallback callback) { impl->dataModifiedCallback = callback; } GraphWidget::GraphWidget(View* parentView) { impl = new GraphWidgetImpl(this, parentView); impl->screen->setBackgroundRole(QPalette::Base); impl->screen->setAutoFillBackground(true); } GraphWidgetImpl::GraphWidgetImpl(GraphWidget* self, View* parentView) : self(self), parentView(parentView) { cursorDashes.push_back(8.0); cursorDashes.push_back(2.0); limitValueDashes.push_back(4.0); limitValueDashes.push_back(4.0); screen = new QWidget(self); screen->setMouseTracking(true); screen->installEventFilter(self); hScrollbar = new DoubleScrollBar(Qt::Horizontal, self); hScrollbar->sigValueChanged().connect( bind(&GraphWidgetImpl::onHScrollbarChanged, this, _1)); vScrollbar = new DoubleScrollBar(Qt::Vertical, self); vScrollbar->sigValueChanged().connect( bind(&GraphWidgetImpl::onVScrollbarChanged, this, _1)); QGridLayout* grid = new QGridLayout(self); grid->setSpacing(0); grid->setContentsMargins(0, 0, 0, 0); grid->addWidget(screen, 0, 0); grid->addWidget(hScrollbar, 1, 0); grid->addWidget(vScrollbar, 0, 1); self->setLayout(grid); isOrgValueVisible = true; isVelocityVisible = false; isAccelerationVisible = false; mode = GraphWidget::VIEW_MODE; editMode = GraphWidget::FREE_LINE_MODE; dragState = DRAG_NONE; screenMarginX = 3.0; scaleX = 200.0; scaleY = 100.0; leftX = 0.0; centerY = 0.0; rangeUpperY = 10.0; rangeLowerY = -rangeUpperY; lineWidth = 1.0; isLimitVisible = true; isGridOn = true; gridWidth = 0.2; gridHeight = 0.2; editTarget = 0; isControlPointsHighlighted = false; controlPointStep = 1; controlPointOffset = 0; isReconfigurationNeeded = false; isDomainUpdateNeeded = true; autoScrollMode = GraphWidget::CONTINUOUS; timeBar = TimeBar::instance(); timeBarSyncMode = true; parentView->sigActivated().connect(bind(&GraphWidgetImpl::onActivated, this)); parentView->sigDeactivated().connect(bind(&GraphWidgetImpl::onDeactivated, this)); statusLabel.setAlignment(Qt::AlignLeft); QFont font = statusLabel.font(); font.setFixedPitch(true); statusLabel.setFont(font); cursorX = -1.0; } GraphWidget::~GraphWidget() { delete impl; } GraphWidgetImpl::~GraphWidgetImpl() { connections.disconnect(); GraphBar::instance()->releaseFocus(self); } void GraphWidgetImpl::onActivated() { enableTimeBarSync(timeBarSyncMode); } void GraphWidgetImpl::onDeactivated() { enableTimeBarSync(false); } void GraphWidget::addDataHandler(GraphDataHandlerPtr handler) { impl->addDataHandler(handler); } void GraphWidgetImpl::addDataHandler(GraphDataHandlerPtr handler) { GraphDataHandlerImpl* data = handler->impl; handlers.push_back(handler); connections.add(handler->impl->sigDataUpdated.connect( bind(&GraphWidgetImpl::updateData, this, handler))); isReconfigurationNeeded = true; isDomainUpdateNeeded = true; updateData(handler); if(!editTarget){ if(data->dataModifiedCallback){ editTarget = data; } } } void GraphWidget::clearDataHandlers() { impl->clearDataHandlers(); } void GraphWidgetImpl::clearDataHandlers() { connections.disconnect(); handlers.clear(); editTarget = 0; isReconfigurationNeeded = true; isDomainUpdateNeeded = true; screen->update(); } void GraphWidgetImpl::updateData(GraphDataHandlerPtr handler) { if(TRACE_FUNCTIONS){ cout << "GraphWidgetImpl::updateData()" << endl; } GraphDataHandlerImpl* data = handler->impl; if(data->prevNumValues != data->numFrames){ isReconfigurationNeeded = true; isDomainUpdateNeeded = true; } if(data->dataRequestCallback){ vector& values = data->values; data->dataRequestCallback(0, data->numFrames, &(values[1])); } screen->update(); } void GraphWidget::setRenderingTypes (bool showOriginalValues, bool showVelocities, bool showAccelerations) { impl->isOrgValueVisible = showOriginalValues; impl->isVelocityVisible = showVelocities; impl->isAccelerationVisible = showAccelerations; impl->screen->update(); } void GraphWidget::getRenderingTypes (bool& showOriginalValues, bool& showVelocities, bool& showAccelerations) { showOriginalValues = impl->isOrgValueVisible; showVelocities = impl->isVelocityVisible; showAccelerations = impl->isAccelerationVisible; } /** @return true if posistion is within the range of the data */ bool GraphWidget::setCursorPosition(double x) { return impl->setCursorPosition(x, false, false); } bool GraphWidgetImpl::setCursorPosition(double x, bool isTimeBarSyncEnabled, bool forceTimeChange) { if(TRACE_FUNCTIONS){ cout << "GraphWidgetImpl::setCursorPosition()" << endl; } bool isWithinDomain = true; if(x < domainLowerX){ x = domainLowerX; isWithinDomain = false; } else if(x > domainUpperX){ x = domainUpperX; isWithinDomain = false; } if(x != cursorX){ bool doScroll = false; const double rightX = leftX + visibleWidth; if(autoScrollMode == GraphWidget::CONTINUOUS){ if(x < leftX){ if(leftX - x < visibleWidth / 3.0){ leftX = x; } else { leftX = x - visibleWidth / 2.0; } doScroll = true; } else if(x >= rightX){ if(x - rightX < visibleWidth / 3.0){ leftX = x - visibleWidth; } else { leftX = x - visibleWidth / 2.0; } doScroll = true; } } else if(autoScrollMode == GraphWidget::PAGE){ if(x < leftX){ if(leftX - x < visibleWidth / 3.0){ leftX -= visibleWidth; } else { leftX = x - visibleWidth / 2.0; } doScroll = true; } else if(x >= rightX){ if(x - rightX < visibleWidth / 3.0){ leftX += visibleWidth; } else { leftX = x - visibleWidth / 2.0; } doScroll = true; } } if(doScroll){ leftX = std::max(domainLowerX, std::min(domainUpperX - visibleWidth, leftX)); screenCursorX = screenMarginX + (x - leftX) * scaleX; isReconfigurationNeeded = true; screen->update(); } else { double oldScreenCursorX = screenCursorX; screenCursorX = screenMarginX + (x - leftX) * scaleX; screen->update((int)oldScreenCursorX - 2, 0, 4, (int)screenHeight); screen->update((int)screenCursorX - 2, 0, 4, (int)screenHeight); } cursorX = x; } if(isTimeBarSyncEnabled && timeBarSyncMode){ double time = cursorX; if(timeBar->time() != time || forceTimeChange){ timeBar->setTime(time); } } return isWithinDomain; } bool GraphWidgetImpl::setCursorPositionWithScreenX(double screenX, bool isTimeBarSyncEnabled, bool forceTimeChange) { return setCursorPosition(leftX + (screenX - screenMarginX) / scaleX, isTimeBarSyncEnabled, forceTimeChange); } void GraphWidget::setTimeBarSyncMode(bool on) { impl->setTimeBarSyncMode(on); } void GraphWidgetImpl::setTimeBarSyncMode(bool on) { timeBarSyncMode = on; enableTimeBarSync(on); } void GraphWidgetImpl::enableTimeBarSync(bool on) { if(TRACE_FUNCTIONS){ cout << "GraphWidgetImpl::enableTimeBarSync()" << endl; } if(timeChangedConnetion.connected()){ if(!on){ timeChangedConnetion.disconnect(); } } else { if(on && parentView->isActive()){ timeChangedConnetion = timeBar->sigTimeChanged().connect( bind(&GraphWidgetImpl::setCursorPosition, this, _1, false, false)); setCursorPosition(timeBar->time(), false, false); } } } bool GraphWidget::isTimeBarSyncMode() { return impl->timeBarSyncMode; } void GraphWidget::setAutoScrollMode(ScrollMode mode) { impl->setAutoScrollMode(mode); } void GraphWidgetImpl::setAutoScrollMode(GraphWidget::ScrollMode mode) { autoScrollMode = mode; } GraphWidget::ScrollMode GraphWidget::autoScrollMode() { return impl->autoScrollMode; } void GraphWidget::setVerticalValueRange(double lower, double upper) { impl->setVerticalValueRange(lower, upper); } void GraphWidgetImpl::setVerticalValueRange(double lower, double upper) { rangeLowerY = lower; rangeUpperY = upper; isReconfigurationNeeded = true; screen->update(); } void GraphWidget::getVerticalValueRange(double& lower, double& upper) { lower = impl->rangeLowerY; upper = impl->rangeUpperY; } void GraphWidget::setLineWidth(double width) { impl->lineWidth = width; impl->screen->update(); } double GraphWidget::getLineWidth() { return impl->lineWidth; } void GraphWidget::showRulers(bool show) { impl->showRulers(show); } void GraphWidgetImpl::showRulers(bool show) { } bool GraphWidget::showsRulers() { return false; } void GraphWidget::showLimits(bool show) { impl->showLimits(show); } void GraphWidgetImpl::showLimits(bool show) { isLimitVisible = show; screen->update(); } bool GraphWidget::showsLimits() { return impl->isLimitVisible; } void GraphWidget::showGrid(bool show) { impl->showGrid(show); } void GraphWidgetImpl::showGrid(bool show) { isGridOn = show; isReconfigurationNeeded = true; screen->update(); } bool GraphWidget::showsGrid() { return impl->isGridOn; } void GraphWidget::setGridSize(double width, double height) { impl->setGridSize(width, height); } void GraphWidgetImpl::setGridSize(double width, double height) { gridWidth = width; gridHeight = height; if(isGridOn){ isReconfigurationNeeded = true; screen->update(); } } void GraphWidget::getGridSize(double& width, double& height) { width = impl->gridWidth; height = impl->gridHeight; } void GraphWidget::setControlPointStep(int step, int offset) { impl->setControlPointStep(step, offset); } void GraphWidgetImpl::setControlPointStep(int step, int offset) { bool changed = false; if(step != controlPointStep){ controlPointStep = step; changed = true; } if(offset != controlPointOffset){ controlPointOffset = offset; changed = true; } if(changed){ for(size_t i=0; i < handlers.size(); ++i){ handlers[i]->impl->isControlPointUpdateNeeded = true; } screen->update(); } } void GraphWidget::getControlPointStep(int& step, int& offset) { step = impl->controlPointStep; offset = impl->controlPointOffset; } void GraphWidget::highlightControlPoints(bool on) { impl->highlightControlPoints(on); } void GraphWidgetImpl::highlightControlPoints(bool on) { isControlPointsHighlighted = on; screen->update(); } bool GraphWidget::highlightsControlPoints() { return impl->isControlPointsHighlighted; } void GraphWidget::changeMode(Mode mode) { impl->changeMode(mode); } void GraphWidgetImpl::changeMode(GraphWidget::Mode mode) { if(mode != this->mode){ if(mode == GraphWidget::EDIT_MODE){ for(size_t i=0; i < handlers.size(); ++i){ updateControlPoints(handlers[i]->impl); } } screen->update(); } } GraphWidget::Mode GraphWidget::mode() { return impl->mode; } void GraphWidget::changeEditMode(EditMode mode) { impl->changeEditMode(mode); } void GraphWidgetImpl::changeEditMode(GraphWidget::EditMode mode) { editMode = mode; } GraphWidget::EditMode GraphWidget::editMode() { return impl->editMode; } QLabel& GraphWidget::statusLabel() { return impl->statusLabel; } void GraphWidgetImpl::setupConfiguration() { if(isDomainUpdateNeeded){ updateDomain(); } visibleWidth = (screenWidth - screenMarginX * 2) / scaleX; double rightX = leftX + visibleWidth; if(rightX > domainUpperX){ leftX -= (rightX - domainUpperX); } if(leftX < domainLowerX){ leftX = domainLowerX; } screenCenterY = screenHeight / 2.0; visibleHeight = screenHeight / scaleY; visibleHeightHalf = visibleHeight / 2.0; topY = centerY - visibleHeightHalf; bottomY = centerY + visibleHeightHalf; hScrollbar->blockSignals(true); if(domainUpperX == domainLowerX){ hScrollbar->setRange(domainLowerX, domainLowerX + 1.0); hScrollbar->setPageStep(1.0); } else { hScrollbar->setRange(domainLowerX, domainUpperX); hScrollbar->setPageStep(visibleWidth); } hScrollbar->setSingleStep(visibleWidth / 100.0); hScrollbar->setValue(leftX); hScrollbar->blockSignals(false); vScrollbar->blockSignals(true); vScrollbar->setRange(rangeLowerY, rangeUpperY); vScrollbar->setPageStep(visibleHeight); vScrollbar->setSingleStep(visibleHeight / 100.0); vScrollbar->setValue(topY); vScrollbar->blockSignals(false); //hRuler.setRange(leftX, rightX, cursorX, std::numeric_limits::max()); //vRuler.setRange(-topY, -bottomY, 0, std::numeric_limits::max()); isReconfigurationNeeded = false; } void GraphWidgetImpl::updateDomain() { double tmpDomainLowerX = std::numeric_limits::max(); double tmpDomainUpperX = -std::numeric_limits::max(); for(size_t i=0; i < handlers.size(); ++i){ GraphDataHandlerImpl* data = handlers[i]->impl; if(data->offset < tmpDomainLowerX){ tmpDomainLowerX = data->offset; } double upper = data->offset + data->numFrames * data->stepRatio; if(upper > tmpDomainUpperX){ tmpDomainUpperX = upper; } } if(tmpDomainLowerX == std::numeric_limits::max()){ domainLowerX = 0.0; // domainUpperX = 10.0; domainUpperX = 0.0; } else { domainLowerX = tmpDomainLowerX; domainUpperX = tmpDomainUpperX; } isDomainUpdateNeeded = false; } bool GraphWidget::eventFilter(QObject* obj, QEvent* event) { if(obj == impl->screen){ switch(event->type()){ case QEvent::FocusIn: return impl->onFocusInEvent(static_cast(event)); case QEvent::Paint: return impl->onScreenPaintEvent(static_cast(event)); case QEvent::Resize: return impl->onScreenResizeEvent(static_cast(event)); case QEvent::MouseMove: return impl->onScreenMouseMoveEvent(static_cast(event)); case QEvent::MouseButtonPress: return impl->onScreenMouseButtonPressEvent(static_cast(event)); case QEvent::MouseButtonRelease: return impl->onScreenMouseButtonReleaseEvent(static_cast(event)); case QEvent::KeyPress: return impl->onScreenKeyPressEvent(static_cast(event)); case QEvent::KeyRelease: return impl->onScreenKeyReleaseEvent(static_cast(event)); default: return false; } } return QWidget::eventFilter(obj, event); } bool GraphWidgetImpl::onFocusInEvent(QFocusEvent* event) { GraphBar::instance()->focus(self); return false; } bool GraphWidgetImpl::onScreenResizeEvent(QResizeEvent* event) { screenWidth = event->size().width(); screenHeight = event->size().height(); setupConfiguration(); return false; } bool GraphWidgetImpl::onScreenMouseButtonPressEvent(QMouseEvent* event) { screen->setFocus(Qt::MouseFocusReason); dragState = DRAG_NONE; pressedScreenX = event->x(); pressedScreenY = event->y(); dragPrevScreenX = event->x(); dragPrevScreenY = event->y(); dragOrgLeftX = leftX; dragOrgCenterY = centerY; if(event->button() == Qt::LeftButton){ if(event->modifiers() & Qt::ControlModifier){ selectEditTargetByClicking(pressedScreenX, pressedScreenY); } else if(mode == GraphWidget::EDIT_MODE && editTarget) { startTrajectoryEdit(); doTrajectoryEdit((int)pressedScreenX, (int)pressedScreenY, event->modifiers()); } else { dragState = DRAG_CURSOR; setCursorPositionWithScreenX(pressedScreenX, true, false); } } else if(event->button() == Qt::MidButton){ dragState = DRAG_ZOOM; dragOrgScaleX = scaleX; dragOrgScaleY = scaleY; } else if(event->button() == Qt::RightButton){ dragState = DRAG_MOVE; } return true; } bool GraphWidgetImpl::onScreenMouseButtonReleaseEvent(QMouseEvent* event) { if(dragState == DRAG_EDIT){ finishTrajectoryEdit(); } dragState = DRAG_NONE; return true; } bool GraphWidgetImpl::onScreenMouseMoveEvent(QMouseEvent* event) { const double screenX = event->x(); const double screenY = event->y(); switch(dragState){ case DRAG_CURSOR: setCursorPositionWithScreenX(screenX, true, false); break; case DRAG_EDIT: doTrajectoryEdit(screenX, screenY, event->modifiers()); break; case DRAG_ZOOM: zoomScreen(screenX, screenY); break; case DRAG_MOVE: moveScreen(screenX, screenY); break; default: break; } double x = leftX + (screenX - screenMarginX) / scaleX; double y = - centerY - (screenY - screenCenterY) / scaleY; //hRuler.property_position() = x; //vRuler.property_position() = y; static format f(_("Graph: Position = (%1$.5f, %2$.5f)")); statusLabel.setText(str(f % x % y).c_str()); dragPrevScreenX = screenX; dragPrevScreenY = screenY; return true; } bool GraphWidgetImpl::onScreenKeyPressEvent(QKeyEvent* event) { bool unprocessed = false; if(dragState == DRAG_EDIT){ switch(event->key()){ case Qt::Key_Shift: doTrajectoryEdit(dragPrevScreenX, dragPrevScreenY, event->modifiers() & Qt::ShiftModifier); break; default: unprocessed = true; break; } } else if(dragState == DRAG_NONE) { switch(event->key()){ case Qt::Key_Z: if(event->modifiers() & Qt::ControlModifier){ if(event->modifiers() & Qt::ShiftModifier){ redoTrajectoryEdit(); } else { undoTrajectoryEdit(); } } break; case Qt::Key_F: changeEditMode(GraphWidget::FREE_LINE_MODE); break; case Qt::Key_L: changeEditMode(GraphWidget::LINE_MODE); break; default: unprocessed = true; break; } } return !unprocessed; } bool GraphWidgetImpl::onScreenKeyReleaseEvent(QKeyEvent* event) { bool unprocessed = false; switch(event->key()){ case Qt::Key_Shift: if(dragState == DRAG_EDIT){ doTrajectoryEdit(dragPrevScreenX, dragPrevScreenY, event->modifiers() & Qt::ShiftModifier); } break; default: unprocessed = true; break; } return unprocessed; } void GraphWidgetImpl::zoomScreen(int screenX, int screenY) { double zoomRatioX = pow(1.01, screenX - pressedScreenX); double zoomRatioY = pow(1.01, pressedScreenY - screenY); scaleX = dragOrgScaleX * zoomRatioX; scaleY = dragOrgScaleY * zoomRatioY; double dpx = (pressedScreenX - screenMarginX) / dragOrgScaleX; double dx = dpx * (zoomRatioX - 1.0) / zoomRatioX; leftX = dragOrgLeftX + dx; if(leftX < domainLowerX){ leftX = domainLowerX; } double dpy = (pressedScreenY - screenCenterY) / dragOrgScaleY; double dy = dpy * (zoomRatioY - 1.0) / zoomRatioY; centerY = dragOrgCenterY + dy; isReconfigurationNeeded = true; screen->update(); } void GraphWidgetImpl::moveScreen(double screenX, double screenY) { leftX = dragOrgLeftX + (pressedScreenX - screenX) / scaleX; centerY = dragOrgCenterY + (pressedScreenY - screenY) / scaleY; isReconfigurationNeeded = true; screen->update(); } void GraphWidgetImpl::startTrajectoryEdit() { if((controlPointStep == 1) || editMode != GraphWidget::FREE_LINE_MODE){ isEditBufferedUpdateMode = timeBar->isDoingPlayback(); editedFrameBegin = std::numeric_limits::max(); editedFrameEnd = std::numeric_limits::min(); editTarget->editHistories.push_back(EditHistoryPtr(new EditHistory())); editTarget->currentHistory = editTarget->editHistories.size(); dragState = DRAG_EDIT; } } void GraphWidgetImpl::finishTrajectoryEdit() { EditHistoryList& histories = editTarget->editHistories; //vector& values = editTarget->values; double* values = &editTarget->values[1]; if(!(editedFrameBegin < editedFrameEnd)){ histories.pop_back(); } else { if(isEditBufferedUpdateMode){ editTarget->dataModifiedCallback (editedFrameBegin, editedFrameEnd - editedFrameBegin, &values[editedFrameBegin]); } // set new values (for redo) to history vector& newValues = histories.back()->newValues; newValues.resize(editedFrameEnd - editedFrameBegin); std::copy(&values[editedFrameBegin], &values[editedFrameEnd], newValues.begin()); while(histories.size() > MAX_NUM_HISTORIES){ histories.pop_front(); editTarget->currentHistory = histories.size(); } } } void GraphWidgetImpl::doTrajectoryEdit(int screenX, int screenY, Qt::KeyboardModifiers modifiers) { static const int FROM = 0; static const int TO = 1; double x[2]; double y[2]; double frameAtX[2]; double flooredFrameAtX[2]; int frameToEdit[2]; int orgScreenX, orgScreenY; if(editMode == GraphWidget::FREE_LINE_MODE){ orgScreenX = dragPrevScreenX; orgScreenY = dragPrevScreenY; } else { orgScreenX = pressedScreenX; orgScreenY = pressedScreenY; } //x[FROM] = leftX + (orgScreenX - screenMarginX) / scaleX; //x[TO] = leftX + (screenX - screenMarginX) / scaleX; x[FROM] = dragOrgLeftX + (orgScreenX - screenMarginX) / scaleX; x[TO] = dragOrgLeftX + (screenX - screenMarginX) / scaleX; y[FROM] = - centerY - (orgScreenY - screenCenterY) / scaleY; y[TO] = - centerY - (screenY - screenCenterY) / scaleY; for(int i=0; i < 2; ++i){ frameAtX[i] = x[i] / editTarget->stepRatio; flooredFrameAtX[i] = floor(frameAtX[i]); frameToEdit[i] = static_cast(flooredFrameAtX[i]) + 1; } bool draggedLeftToRight = (x[FROM] <= x[TO]); int LEFT = draggedLeftToRight ? 0 : 1; int RIGHT = draggedLeftToRight ? 1 : 0; int& frameBegin = frameToEdit[LEFT]; int& frameEnd = frameToEdit[RIGHT]; bool inAdjacentFrames = (frameBegin == frameEnd); if(inAdjacentFrames){ double odd = frameAtX[TO] - flooredFrameAtX[TO]; if(odd >= 0.5){ frameEnd++; } else if(odd < 0.5){ frameBegin--; } } if(editMode == GraphWidget::LINE_MODE && !inAdjacentFrames){ if(draggedLeftToRight){ if(frameAtX[LEFT] - flooredFrameAtX[LEFT] >= 0.5){ frameBegin++; } if(frameAtX[RIGHT] - flooredFrameAtX[RIGHT] >= 0.5){ frameEnd++; } } else { if(frameAtX[LEFT] - flooredFrameAtX[LEFT] < 0.5){ frameBegin--; } if(frameAtX[RIGHT] - flooredFrameAtX[RIGHT] < 0.5){ frameEnd--; } } } for(int i=0; i < 2; ++i){ if(frameToEdit[i] < 0){ frameToEdit[i] = 0; } } //vector& values = editTarget->values; double* values = &editTarget->values[1]; const int numFrames = editTarget->numFrames; if(frameEnd > numFrames){ frameEnd = numFrames; } if(editMode == GraphWidget::LINE_MODE){ EditHistoryPtr& history = editTarget->editHistories.back(); std::copy(history->orgValues.begin(), history->orgValues.end(), &values[history->frame]); } if(frameBegin < frameEnd){ storeOrgValuesToHistory(frameBegin, frameEnd); if(inAdjacentFrames){ values[frameBegin] = min(max(y[TO], editTarget->lowerValueLimit), editTarget->upperValueLimit); } else if(editMode == GraphWidget::FREE_LINE_MODE){ double k = (y[RIGHT] - y[LEFT]) / (frameAtX[RIGHT] - frameAtX[LEFT]); for(int frame = frameBegin; frame < frameEnd; ++frame){ double v = k * (frame - frameAtX[LEFT]) + y[LEFT]; values[frame] = min(max(v, editTarget->lowerValueLimit), editTarget->upperValueLimit); } } else if(editMode == GraphWidget::LINE_MODE){ double k; double y0; int frame0; if(modifiers & Qt::ShiftModifier){ k = 0.0; y0 = y[TO]; frame0 = frameBegin; } else { if(draggedLeftToRight){ frame0 = frameBegin - 1; } else { frame0 = frameBegin; } k = (y[RIGHT] - y[LEFT]) / (frameEnd - frameBegin); y0 = y[LEFT]; } for(int frame = frameBegin; frame < frameEnd; ++frame){ double v = k * (frame - frame0) + y0; values[frame] = min(max(v, editTarget->lowerValueLimit), editTarget->upperValueLimit); } } editedFrameBegin = std::min(editedFrameBegin, frameBegin); editedFrameEnd = std::max(editedFrameEnd, frameEnd); if(!isEditBufferedUpdateMode){ editTarget->dataModifiedCallback(frameBegin, frameEnd - frameBegin, &values[frameBegin]); } } screen->update(); double cursorX; if(TO==RIGHT && frameToEdit[LEFT] < frameToEdit[RIGHT]){ cursorX = (frameToEdit[TO] - 1.0 + 0.000001) * editTarget->stepRatio; } else { cursorX = (frameToEdit[TO] + 0.000001) * editTarget->stepRatio; } setCursorPosition(cursorX, true, true); } void GraphWidgetImpl::storeOrgValuesToHistory(int frameBegin, int frameEnd) { //vector& values = editTarget->values; double* values = &editTarget->values[1]; EditHistoryPtr& history = editTarget->editHistories.back(); vector& storedValues = history->orgValues; int nStoredValues = storedValues.size(); if(!nStoredValues){ history->frame = frameBegin; } int leftPartBegin = std::min(frameBegin, history->frame); int leftPartEnd = std::min(frameEnd, history->frame); int rightPartBegin = std::max(frameBegin, history->frame + nStoredValues); int rightPartEnd = std::max(frameEnd, history->frame + nStoredValues); history->frame = leftPartBegin; storedValues.resize(rightPartEnd - leftPartBegin); if(leftPartBegin < leftPartEnd){ std::copy(&values[leftPartBegin], &values[leftPartEnd], storedValues.begin()); } if(rightPartBegin < rightPartEnd){ std::copy(&values[rightPartBegin], &values[rightPartEnd], storedValues.begin() + (rightPartBegin - history->frame)); } } void GraphWidgetImpl::undoTrajectoryEdit() { if(dragState == DRAG_NONE && editTarget){ int& currentHistory = editTarget->currentHistory; if(currentHistory > 0){ currentHistory--; EditHistoryPtr history = editTarget->editHistories[currentHistory]; std::copy(history->orgValues.begin(), history->orgValues.end(), editTarget->values.begin() + history->frame + 1); editTarget->dataModifiedCallback(history->frame, history->orgValues.size(), &history->orgValues[0]); screen->update(); } } } void GraphWidgetImpl::redoTrajectoryEdit() { if(dragState == DRAG_NONE && editTarget){ EditHistoryList& histories = editTarget->editHistories; int& currentHistory = editTarget->currentHistory; if(currentHistory < static_cast(histories.size())){ EditHistoryPtr history = editTarget->editHistories[currentHistory]; std::copy(history->newValues.begin(), history->newValues.end(), editTarget->values.begin() + history->frame + 1); editTarget->dataModifiedCallback(history->frame, history->newValues.size(), &history->newValues[0]); currentHistory++; screen->update(); } } } void GraphWidgetImpl::selectEditTargetByClicking(double screenX, double screenY) { static const double pixelMargin = 6.0; GraphDataHandlerImpl* newTarget = 0; double pointerX = leftX + (screenX - screenMarginX) / scaleX; double pointerY = - centerY - (screenY - screenCenterY) / scaleY; double yMargin = pixelMargin / scaleY; for(size_t i=0; i < handlers.size(); ++i){ GraphDataHandlerImpl* data = handlers[i]->impl; double* values = &data->values[1]; const int size = data->numFrames; int frameAtPointer = (int)floor(pointerX / data->stepRatio); int frameMargin = (int)std::max(pixelMargin / scaleX / data->stepRatio, 1.0); int frameBegin = std::max(frameAtPointer - frameMargin, 0); int frameEnd = std::min(frameAtPointer + frameMargin, size); double yMin = std::numeric_limits::max(); double yMax = -std::numeric_limits::max(); for(int j=frameBegin; j < frameEnd; ++j){ double y = values[j]; yMin = std::min(yMin, y - yMargin); yMax = std::max(yMax, y + yMargin); } if((pointerY >= yMin) && (pointerY <= yMax)){ newTarget = data; break; } } // unselect operation if(newTarget == editTarget){ newTarget = 0; } GraphWidget::Mode newMode = newTarget ? GraphWidget::EDIT_MODE : GraphWidget::VIEW_MODE; if(newMode != mode){ editTarget = newTarget; changeMode(newMode); } else { if(newTarget != editTarget){ screen->update(); } editTarget = newTarget; } } bool GraphWidgetImpl::onScreenPaintEvent(QPaintEvent* event) { if(isReconfigurationNeeded){ setupConfiguration(); } QPainter painter(screen); //painter.setRenderHint(QPainter::Antialiasing); painter.setClipRegion(event->region()); painter.setClipping(true); pen.setStyle(Qt::SolidLine); pen.setWidthF(lineWidth); painter.setPen(pen); // draw the grid if(isGridOn){ drawGrid(painter); } // draw x-axis pen.setColor(QColor(0, 0, 0)); painter.setPen(pen); double ay = screenCenterY - centerY * scaleY; painter.drawLine(QPointF(0.0, ay), QPointF(screenWidth, ay)); if(isLimitVisible){ for(size_t i=0; i < handlers.size(); ++i){ drawLimits(painter, handlers[i]->impl); } } for(size_t i=0; i < handlers.size(); ++i){ GraphDataHandlerImpl* data = handlers[i]->impl; drawTrajectory(painter, event->rect(), data); } // draw the cursor pen.setColor(QColor(25, 25, 25, 230)); pen.setDashPattern(cursorDashes); painter.setPen(pen); painter.drawLine(QPointF(screenCursorX, 0.0), QPointF(screenCursorX, screenHeight)); //return true; return false; } #include static inline double calcVelocity(int frame, double* values, double stepRatio2) { return (values[frame + 1] - values[frame - 1]) / stepRatio2; } void GraphWidgetImpl::drawTrajectory (QPainter& painter, const QRect& rect, GraphDataHandlerImpl* data) { double areaLeft = rect.x(); double areaRight = rect.x() + rect.width(); if(areaLeft < screenMarginX){ areaLeft = screenMarginX; } if(areaRight > screenWidth - screenMarginX){ areaRight = screenWidth - screenMarginX; } double clippedLeftX = leftX + (areaLeft - screenMarginX) / scaleX; int frame_begin = (int)floor(clippedLeftX / data->stepRatio); const double xratio = data->stepRatio * scaleX; double interFrameOffset = fmod(clippedLeftX / data->stepRatio, 1.0) * xratio; double screenOffsetX = areaLeft - interFrameOffset; double clippedRightX = leftX + (areaRight - screenMarginX) / scaleX; int frame_end = (int)floor(clippedRightX / data->stepRatio) + 2; //vector& values = data->values; double* values = &data->values[1]; const int numFrames = data->numFrames; if(frame_end > numFrames){ frame_end = numFrames; } if(frame_begin < frame_end){ QColor color; if(isVelocityVisible){ values[-1] = values[0]; values[numFrames] = values[numFrames - 1]; const double stepRatio2 = 2.0 * data->stepRatio; int frame = frame_begin; if(frame < 0){ frame = 0; } color.setRgbF((data->g + 0.5) / 2.0, (data->b + 0.2) / 2.0, data->r); pen.setColor(color); painter.setPen(pen); if(xratio > 0.2){ const int n = frame_end - frame; polyline.resize(n); for(int i=0; i < n; ++i){ const double px = screenOffsetX + (frame - frame_begin) * xratio; const double v = calcVelocity(frame, values, stepRatio2); const double py = screenCenterY - (v + centerY) * scaleY; polyline[i] = QPointF(px, py); ++frame; } } else { const int m = (int)(0.5 / xratio); const int n = ceil(double(frame_end - frame) / m); polyline.resize(n * 2); for(int i=0; i < n; ++i){ double px_min = screenOffsetX + (frame - frame_begin) * xratio; double px_max = px_min; const int next = std::min(frame + m, frame_end); double min = calcVelocity(frame++, values, stepRatio2); double max = min; while(frame < next){ const double v = calcVelocity(frame, values, stepRatio2); if(v > max){ max = v; px_max = screenOffsetX + (frame - frame_begin) * xratio; } else if(v < min){ min = v; px_min = screenOffsetX + (frame - frame_begin) * xratio; } frame++; } const double upper = screenCenterY - (max + centerY) * scaleY; const double lower = screenCenterY - (min + centerY) * scaleY; if(px_min <= px_max){ polyline[i*2] = QPointF(px_min, lower); polyline[i*2+1] = QPointF(px_max, upper); } else { polyline[i*2] = QPointF(px_max, upper); polyline[i*2+1] = QPointF(px_min, lower); } } } painter.drawPolyline(polyline); } if(isOrgValueVisible){ int frame = frame_begin; if(frame < 0){ frame = 0; } color.setRgbF(data->r, data->g, data->b); pen.setColor(color); painter.setPen(pen); if(xratio > 0.2){ const int n = frame_end - frame; polyline.resize(n); for(int i=0; i < n; ++i){ const double px = screenOffsetX + (frame - frame_begin) * xratio; const double py = screenCenterY - (values[frame] + centerY) * scaleY; polyline[i] = QPointF(px, py); ++frame; } } else { const int m = (int)(0.5 / xratio); const int n = ceil(double(frame_end - frame) / m); polyline.resize(n * 2); for(int i=0; i < n; ++i){ double px_min = screenOffsetX + (frame - frame_begin) * xratio; double px_max = px_min; const int next = std::min(frame + m, frame_end); double min = values[frame++]; double max = min; while(frame < next){ const double v = values[frame++]; if(v > max){ max = v; px_max = screenOffsetX + (frame - frame_begin) * xratio; } else if(v < min){ min = v; px_min = screenOffsetX + (frame - frame_begin) * xratio; } } const double upper = screenCenterY - (max + centerY) * scaleY; const double lower = screenCenterY - (min + centerY) * scaleY; if(px_min <= px_max){ polyline[i*2] = QPointF(px_min, lower); polyline[i*2+1] = QPointF(px_max, upper); } else { polyline[i*2] = QPointF(px_max, upper); polyline[i*2+1] = QPointF(px_min, lower); } } } painter.drawPolyline(polyline); if(isControlPointsHighlighted || (mode == GraphWidget::EDIT_MODE && data == editTarget)){ updateControlPoints(data); int frame = frame_begin; if(frame < 0){ frame = 0; } dynamic_bitset<>& mask = data->controlPointMask; while(frame < frame_end){ if(mask[frame]){ double px = screenOffsetX + (frame - frame_begin) * xratio; double py = screenCenterY - (values[frame] + centerY) * scaleY; painter.fillRect(QRectF(px - 2, py - 2, 4, 4), color); } ++frame; } } } } } void GraphWidgetImpl::updateControlPoints(GraphDataHandlerImpl* data) { if(data->isControlPointUpdateNeeded){ data->isControlPointUpdateNeeded = false; const int numFrames = data->numFrames; data->controlPointMask.resize(numFrames); for(int i=0; i < numFrames; ++i){ data->controlPointMask[i] = (i % controlPointStep == controlPointOffset); } } } void GraphWidgetImpl::drawLimits(QPainter& painter, GraphDataHandlerImpl* data) { pen.setDashPattern(limitValueDashes); QColor color; if(isVelocityVisible){ color.setRgbF((data->g + 0.5) / 2.0, (data->b + 0.2) / 2.0, data->r); pen.setColor(color); painter.setPen(pen); double lower = screenCenterY - (data->lowerVelocityLimit + centerY) * scaleY; if(isfinite(lower)){ painter.drawLine(QPointF(screenMarginX, lower), QPointF(screenWidth - screenMarginX, lower)); } double upper = screenCenterY - (data->upperVelocityLimit + centerY) * scaleY; if(isfinite(upper)){ painter.drawLine(QPointF(screenMarginX, upper), QPointF(screenWidth - screenMarginX, upper)); } } if(isOrgValueVisible){ color.setRgbF(data->r, data->g, data->b); pen.setColor(color); painter.setPen(pen); double lower = screenCenterY - (data->lowerValueLimit + centerY) * scaleY; if(isfinite(lower)){ painter.drawLine(QPointF(screenMarginX, lower), QPointF(screenWidth - screenMarginX, lower)); } double upper = screenCenterY - (data->upperValueLimit + centerY) * scaleY; if(isfinite(upper)){ painter.drawLine(QPointF(screenMarginX, upper), QPointF(screenWidth - screenMarginX, upper)); } } pen.setStyle(Qt::SolidLine); } void GraphWidgetImpl::drawGrid(QPainter& painter) { QColor color; color.setRgbF(0.8, 0.8, 0.8); pen.setColor(color); painter.setPen(pen); double x = leftX - fmod(leftX, gridWidth); while(x <= leftX + visibleWidth){ double px = (x - leftX) * scaleX + screenMarginX; painter.drawLine(QPointF(px, 0.0), QPointF(px, screenHeight)); x += gridWidth; } double y = topY - fmod(topY, gridHeight); while(y <= bottomY){ double py = (y - topY) * scaleY; painter.drawLine(QPointF(screenMarginX, py), QPointF(screenWidth - screenMarginX, py)); y += gridHeight; } } void GraphWidgetImpl::onHScrollbarChanged(double value) { if(value != leftX){ leftX = value; isReconfigurationNeeded = true; screen->update(); } } void GraphWidgetImpl::onVScrollbarChanged(double value) { double y = value + visibleHeightHalf; if(y != centerY){ centerY = y; isReconfigurationNeeded = true; screen->update(); } } bool GraphWidget::storeState(Archive& archive) { return impl->storeState(archive); } bool GraphWidgetImpl::storeState(Archive& archive) { static const char* modeLabel[] = { "view", "edit" }; archive.write("mode", modeLabel[mode]); static const char* editModeLabel[] = { "freeLine", "line" }; archive.write("editMode", editModeLabel[editMode]); archive.write("original", isOrgValueVisible); archive.write("velocity", isVelocityVisible); archive.write("acceleration", isAccelerationVisible); archive.write("limits", isLimitVisible); archive.write("grid", isGridOn); archive.write("gridWidth", gridWidth); archive.write("gridHeight", gridHeight); archive.write("lineWidth", lineWidth); archive.write("rulers", self->showsRulers()); archive.write("sync", timeBarSyncMode); archive.write("controlPointStep", controlPointStep); archive.write("controlPointOffset", controlPointOffset); archive.write("controlPointHeighlight", isControlPointsHighlighted); static const char* scrollModeLabel[] = { "off", "continuous", "page" }; archive.write("scrollMode", scrollModeLabel[autoScrollMode]); archive.write("lower", rangeLowerY); archive.write("upper", rangeUpperY); return true; } bool GraphWidget::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool GraphWidgetImpl::restoreState(const Archive& archive) { string modeLabel; if(archive.read("mode", modeLabel)){ changeMode((modeLabel == "edit") ? GraphWidget::EDIT_MODE : GraphWidget::VIEW_MODE); } string editModeLabel; if(archive.read("editMode", editModeLabel)){ if(editModeLabel == "freeLine"){ editMode = GraphWidget::FREE_LINE_MODE; } else if(editModeLabel == "line"){ editMode = GraphWidget::LINE_MODE; } } archive.read("original", isOrgValueVisible); archive.read("velocity", isVelocityVisible); archive.read("acceleration", isAccelerationVisible); archive.read("limits", isLimitVisible); archive.read("grid", isGridOn); archive.read("gridWidth", gridWidth); archive.read("gridHeight", gridHeight); archive.read("lineWidth", lineWidth); self->showRulers(archive.get("rulers", self->showsRulers())); setTimeBarSyncMode(archive.get("sync", timeBarSyncMode)); int step = archive.get("controlPointStep", controlPointStep); int offset = archive.get("controlPointOffset", controlPointOffset); setControlPointStep(step, offset); archive.read("controlPointHeighlight", isControlPointsHighlighted); string scrollModeLabel; if(archive.read("scrollMoe", scrollModeLabel)){ if(scrollModeLabel == "off"){ setAutoScrollMode(GraphWidget::OFF); } else if(scrollModeLabel == "continuous"){ setAutoScrollMode(GraphWidget::CONTINUOUS); } else if(scrollModeLabel == "page"){ setAutoScrollMode(GraphWidget::PAGE); } } archive.read("lower", rangeLowerY); archive.read("upper", rangeUpperY); isReconfigurationNeeded = true; screen->update(); if(GraphBar::instance()->focusedGraphWidget() == self){ GraphBar::instance()->focus(self, true); } return true; } choreonoid-1.1.0+dfsg/src/Base/GraphWidget.h000066400000000000000000000072041207742442300206100ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_GRAPH_WIDGET_H_INCLUDED #define CNOID_GUIBASE_GRAPH_WIDGET_H_INCLUDED #include #include #include #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class View; class ToolBar; class Archive; class GraphDataHandler; class GraphDataHandlerImpl; class GraphWidgetImpl; typedef boost::shared_ptr GraphDataHandlerPtr; class CNOID_EXPORT GraphDataHandler { public: GraphDataHandler(); ~GraphDataHandler(); void setColor(float r, float g, float b); void setLabel(const std::string& label); void setFrameProperties(int numFrames, double frameRate, double offset = 0.0); void setValueLimits(double lower, double upper); void setVelocityLimits(double lower, double upper); void addVerticalLine(double x, const std::string& label); void addHorizontalLine(double y, const std::string& label); void clearLines(); void update(); typedef boost::function DataRequestCallback; void setDataRequestCallback(DataRequestCallback callback); typedef boost::function DataModifiedCallback; void setDataModifiedCallback(DataModifiedCallback callback); private: friend class GraphWidgetImpl; GraphDataHandlerImpl* impl; }; class CNOID_EXPORT GraphWidget : public QWidget, public boost::signals::trackable { public: GraphWidget(View* parentView); ~GraphWidget(); void addDataHandler(GraphDataHandlerPtr handler); void clearDataHandlers(); void setRenderingTypes(bool showOriginalValues, bool showVelocities, bool showAccelerations); void getRenderingTypes(bool& showOriginalValues, bool& showVelocities, bool& showAccelerations); bool setCursorPosition(double pos); void setTimeBarSyncMode(bool on); bool isTimeBarSyncMode(); enum ScrollMode { OFF, CONTINUOUS, PAGE }; void setAutoScrollMode(ScrollMode on); ScrollMode autoScrollMode(); void setVerticalValueRange(double lower, double upper); void getVerticalValueRange(double& lower, double& upper); void setLineWidth(double width); double getLineWidth(); void showRulers(bool show); bool showsRulers(); void showLimits(bool show); bool showsLimits(); void showGrid(bool show); bool showsGrid(); void setGridSize(double width, double height); void getGridSize(double& width, double& height); void setControlPointStep(int step, int offset = 0); void getControlPointStep(int& step, int& offset); void highlightControlPoints(bool on); bool highlightsControlPoints(); enum Mode { VIEW_MODE, EDIT_MODE }; void changeMode(Mode mode); Mode mode(); enum EditMode { FREE_LINE_MODE, LINE_MODE }; void changeEditMode(EditMode mode); EditMode editMode(); QLabel& statusLabel(); virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); protected: virtual bool eventFilter(QObject* obj, QEvent* event); private: friend class GraphDataHandler; GraphWidgetImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/InfoBar.cpp000066400000000000000000000044131207742442300202550ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "InfoBar.h" #include "View.h" #include #include using namespace std; using namespace cnoid; InfoBar* InfoBar::instance() { static InfoBar* infoBar = new InfoBar(); return infoBar; } InfoBar::InfoBar() { indicatorBase = new QWidget(this); indicatorLayout = new QHBoxLayout(); indicatorLayout->setContentsMargins(0, 0, 0, 0); indicatorLayout->setSpacing(0); indicatorBase->setLayout(indicatorLayout); addPermanentWidget(indicatorBase); currentIndicator = 0; connect(qApp, SIGNAL(focusChanged(QWidget*, QWidget*)), this, SLOT(onFocusChanged(QWidget*, QWidget*))); } InfoBar::~InfoBar() { removeCurrentIndicator(); } void InfoBar::notify(const char* message) { showMessage(message, 5000); } void InfoBar::notify(const std::string& message) { showMessage(QString(message.c_str()), 5000); } void InfoBar::notify(const QString& message) { showMessage(message, 5000); } void InfoBar::setIndicator(QWidget* indicator) { if(indicator != currentIndicator){ removeCurrentIndicator(); if(indicator){ indicatorLayout->addWidget(indicator); indicator->show(); connect(indicator, SIGNAL(destroyed(QObject*)), this, SLOT(onIndicatorDestroyed(QObject*))); currentIndicator = indicator; } } } void InfoBar::removeCurrentIndicator() { if(currentIndicator){ indicatorLayout->removeWidget(currentIndicator); currentIndicator->hide(); currentIndicator->setParent(0); currentIndicator->disconnect(SIGNAL(destroyed(QObject*)), this, SLOT(onIndicatorDestroyed(QObject*))); currentIndicator = 0; } } void InfoBar::onIndicatorDestroyed(QObject* obj) { if(obj == currentIndicator){ currentIndicator = 0; } } void InfoBar::onFocusChanged(QWidget* old, QWidget* now) { QWidget* widget = now; while(widget){ View* view = dynamic_cast(widget); if(view){ QWidget* indicator = view->indicatorOnInfoBar(); if(indicator){ setIndicator(indicator); break; } } widget = widget->parentWidget(); } } choreonoid-1.1.0+dfsg/src/Base/InfoBar.h000066400000000000000000000016331207742442300177230ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_INFO_BAR_H_INCLUDED #define CNOID_GUIBASE_INFO_BAR_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { class InfoBarImpl; class CNOID_EXPORT InfoBar : public QStatusBar { Q_OBJECT public: static InfoBar* instance(); void notify(const char* message); void notify(const std::string& message); void notify(const QString& message); private: InfoBar(); ~InfoBar(); QWidget* indicatorBase; QHBoxLayout* indicatorLayout; QWidget* currentIndicator; void setIndicator(QWidget* indicator); void removeCurrentIndicator(); private Q_SLOTS: void onFocusChanged(QWidget* old, QWidget* now); void onIndicatorDestroyed(QObject* obj); }; } #endif choreonoid-1.1.0+dfsg/src/Base/Item.cpp000066400000000000000000000343361207742442300176420ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "Item.h" #include "RootItem.h" #include "ItemPath.h" #include "ItemManager.h" #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; //tmp #include namespace { const bool TRACE_FUNCTIONS = false; } namespace cnoid { boost::signal Item::sigClassUnregistered_; } Item::Item() { init(); } Item::Item(const std::string& name) : name_(name) { init(); } Item::Item(const Item& org) : name_(org.name_) { init(); attributes = org.attributes; } void Item::init() { parent_ = 0; prevItem_ = 0; lastChild_ = 0; attributes.set(); // set all the bits isSubItem_ = false; isConsistentWithLastAccessedFile_ = false; timeStampOfLastFileWriting_ = 0; } Item::~Item() { if(TRACE_FUNCTIONS){ cout << "Item::~Item() of " << name_ << endl; } } /** @if jp アイテムã®åå‰ã‚’設定ï¼å¤‰æ›´ã™ã‚‹ã€‚ åå‰ãŒå¤‰ã‚ã‚‹ã¨ã€sigNameChanged シグナルãŒç™ºè¡Œã•ã‚Œã‚‹ã€‚ @endif */ void Item::setName(const std::string& name) { if(name != name_){ name_ = name; sigNameChanged_(); } } /* Item* Item::child(int index) const { Item* found = 0; int currentIndex = 0; for(Item* item = child(); item; item = item->sibling()){ if(currentIndex == index){ found = item; break; } ++currentIndex; } return found; } */ /** Returns positions in the parent item. */ /* int Item::localIndex() const { int index = -1; if(parent_){ index = 0; Item* item = parent()->child(); while(true){ if(!item){ throw "Error in Item::localIndex()"; } if(item == this){ break; } item = item->sibling(); ++index; } } return index; } */ /* int Item::numChildren() const { int n = 0; for(Item* item = child(); item; item = item->sibling()){ ++n; } return n; } */ /** @if jp 本アイテム以下ã®ãƒ„リーã«æ–°ãŸã«ã‚¢ã‚¤ãƒ†ãƒ ã‚’追加ã™ã‚‹ã€‚ @param item 追加ã™ã‚‹ã‚¢ã‚¤ãƒ†ãƒ  @endif */ void Item::addChildItem(ItemPtr item) { doInsertChildItem(item.get(), 0); } /** @if jp アイテムãŒãã®ä¸å¯æ¬ ãªæ§‹æˆè¦ç´ ã¨ã—ã¦å°ã‚¢ã‚¤ãƒ†ãƒ ã‚’æŒã¤å ´åˆã€ addChildItemã§ã¯ãªã本関数を用ã„ã¦å°ã‚¢ã‚¤ãƒ†ãƒ ã‚’追加ã—ã¦ãŠãã“ã¨ã§ã€ システムã¯ãã®çŠ¶æ³ã‚’把æ¡å¯èƒ½ã¨ãªã‚‹ã€‚ ã“ã®é–¢æ•°ã«ã‚ˆã£ã¦è¿½åŠ ã•ã‚ŒãŸã‚¢ã‚¤ãƒ†ãƒ ã¯ isSubItem() ㌠true ã¨ãªã‚‹ã€‚ @endif */ void Item::addSubItem(ItemPtr item) { item->isSubItem_ = true; addChildItem(item); } void Item::insertChildItem(ItemPtr item, ItemPtr nextItem) { doInsertChildItem(item.get(), nextItem.get()); } void Item::doInsertChildItem(Item* item, Item* nextItem) { bool isMoving = false; RootItem* rootItem = findRootItem(); if(item->parent_){ RootItem* srcRootItem = item->parent_->findRootItem(); if(srcRootItem){ if(srcRootItem == rootItem){ isMoving = true; } } item->detachFromParentItemSub(isMoving); } item->parent_ = this; if(nextItem && (nextItem->parent_ == this)){ Item* prevItem = nextItem->prevItem_; if(prevItem){ prevItem->nextItem_ = item; item->prevItem_ = prevItem; } else { firstChild_ = item; item->prevItem_ = 0; } nextItem->prevItem_ = item; item->nextItem_ = nextItem; } else if(lastChild_){ lastChild_->nextItem_ = item; item->prevItem_ = lastChild_; item->nextItem_ = 0; lastChild_ = item; } else { firstChild_ = item; lastChild_ = item; } // This must be before rootItem->notifyEventOnItemAdded(). item->callSlotsOnPositionChanged(); if(rootItem){ rootItem->notifyEventOnItemAdded(item); if(!isMoving){ item->callFuncOnConnectedToRoot(); } } } void Item::callSlotsOnPositionChanged() { onPositionChanged(); sigPositionChanged_(); for(ItemPtr child = childItem(); child; child = child->nextItem()){ child->callSlotsOnPositionChanged(); } } void Item::callFuncOnConnectedToRoot() { onConnectedToRoot(); for(ItemPtr child = childItem(); child; child = child->nextItem()){ child->callFuncOnConnectedToRoot(); } } bool Item::isSubItem() const { return isSubItem_; } /** @if jp アイテムを親アイテムã‹ã‚‰åˆ‡ã‚Šé›¢ã™ã€‚ @return ルートアイテムã®ãƒ„リー内ã‹ã‚‰åˆ‡ã‚Šé›¢ã•ã‚Œã‚‹å ´åˆã¯ã€ãã®ãƒ«ãƒ¼ãƒˆã‚¢ã‚¤ãƒ†ãƒ ã‚’è¿”ã™ã€‚ @endif */ void Item::detachFromParentItem() { ItemPtr self = this; detachFromParentItemSub(false); } void Item::detachFromParentItemSub(bool isMoving) { RootItem* rootItem = findRootItem(); if(rootItem){ rootItem->notifyEventOnItemRemoving(this, isMoving); } if(parent_){ if(prevItem_){ prevItem_->nextItem_ = nextItem_; } else { parent_->firstChild_ = nextItem_; } if(nextItem_){ nextItem_->prevItem_ = prevItem_; } else { parent_->lastChild_ = prevItem_; } prevItem_ = 0; nextItem_ = 0; parent_ = 0; } isSubItem_ = false; if(rootItem){ rootItem->notifyEventOnItemRemoved(this, isMoving); if(!isMoving){ emitSigDetachedFromRootForSubTree(); } } } void Item::emitSigDetachedFromRootForSubTree() { for(ItemPtr child = childItem(); child; child = child->nextItem()){ child->emitSigDetachedFromRootForSubTree(); } sigDetachedFromRoot_(); onDisconnectedFromRoot(); } void Item::onConnectedToRoot() { } void Item::onDisconnectedFromRoot() { } void Item::onPositionChanged() { } namespace { Item* findItemSub(Item* current, ItemPath::iterator it, ItemPath::iterator end) { if(it == end){ return current; } Item* item = 0; for(Item* child = current->childItem(); child; child = child->nextItem()){ if(child->name() == *it){ item = findItemSub(child, ++it, end); if(item){ break; } } } return item; } } Item* Item::findItem(const std::string& path) const { ItemPath ipath(path); return findItemSub(const_cast(this), ipath.begin(), ipath.end()); } void Item::pickSubItems(Item* item, ItemListBase& itemList) { if(item){ itemList.appendIfTypeMatches(item); pickSubItems(item->childItem(), itemList); pickSubItems(item->nextItem(), itemList); } } RootItem* Item::findRootItem() const { Item* current = const_cast(this); while(current->parent_){ current = current->parent_; } return dynamic_cast(current); } /** @return When the item is embeded one, this function returs the first parent item which is not an embeded one. Otherwise the item itself is returned. */ Item* Item::headItem() const { Item* head = const_cast(this); while(head->isSubItem_){ if(head->parent_){ head = head->parent_; } else { break; } } return head; } void Item::traverse(boost::function function) { traverse(this, function); } void Item::traverse(Item* item, const boost::function& function) { function(item); for(Item* child = item->childItem(); child; child = child->nextItem()){ traverse(child, function); } } /** @todo added the 'notifyUpdateLater()' method ? */ void Item::notifyUpdate() { sigUpdated_(); } /** @if jp アイテムã®ã‚³ãƒ”ーを生æˆã™ã‚‹ã€‚ å°ã‚¢ã‚¤ãƒ†ãƒ ã«ã¤ã„ã¦ã¯ isFixedToParentItem() ㌠true ã®ã¨ãã¯ã‚³ãƒ”ーã•ã‚Œã‚‹ãŒã€ false ã®ã¨ãã¯ã‚³ãƒ”ーã•ã‚Œãªã„。 @endif */ ItemPtr Item::duplicate() const { ItemPtr duplicated = doDuplicate(); if(duplicated && (typeid(*duplicated) != typeid(*this))){ duplicated = 0; } return duplicated; } /** @if jp å°ã‚¢ã‚¤ãƒ†ãƒ ï¼ˆã‚µãƒ–ツリー）もå«ã‚ãŸã‚¢ã‚¤ãƒ†ãƒ ã®ã‚³ãƒ”ーを生æˆã™ã‚‹ã€‚ @endif */ ItemPtr Item::duplicateAll() const { return duplicateAllSub(0); } ItemPtr Item::duplicateAllSub(ItemPtr duplicated) const { if(!duplicated){ duplicated = this->duplicate(); } if(duplicated){ for(ItemPtr child = childItem(); child; child = child->nextItem()){ ItemPtr duplicatedChildItem; if(child->isSubItem()){ duplicatedChildItem = duplicated->findItem(child->name()); if(duplicatedChildItem){ child->duplicateAllSub(duplicatedChildItem); } } else { duplicatedChildItem = child->duplicateAllSub(0); if(duplicatedChildItem){ duplicated->addChildItem(duplicatedChildItem); } } } } return duplicated; } /** 継承クラスã®å®Ÿè£…ã«ãŠã„ã¦ã“ã®é–¢æ•°ã‚’オーãƒãƒ¼ãƒ©ã‚¤ãƒ‰ã™ã‚‹ã€‚ */ ItemPtr Item::doDuplicate() const { return new Item(*this); } /** This function loads the data of the item from a file by using a pre-registered loading function. To make this function available, a loading function has to be registered to an ItemManager in advance by calling the addLoader() or addLoaderAndSaver() function. Otherwise, this function cannot be used. Note that this function should not be overloaded or overridden in the derived classes. */ bool Item::load(const std::string& filename, const std::string& formatId) { return ItemManager::load(this, filename, parentItem(), formatId); } /** @param parentItem specify this when the item is newly created one and will be attached to a parent item if loading succeeds. */ bool Item::load(const std::string& filename, Item* parent, const std::string& formatId) { return ItemManager::load(this, filename, parent, formatId); } bool Item::reload() { if(!lastAccessedFileName_.empty() && !lastAccessedFileFormatId_.empty()){ return load(lastAccessedFileName_, lastAccessedFileFormatId_); } return false; } /** This function saves the data of the item to a file by using a pre-registered saving function. To make this function available, a saving function has to be registered to an ItemManager in advance by calling the addSaver() or addLoaderAndSaver() function. Otherwise, this function cannot be used. Note that this function should not be overloaded or overridden in the derived classes. */ bool Item::save(const std::string& filename, const std::string& formatId) { return ItemManager::save(this, filename, formatId); } /** This function save the data of the item to the file from which the data of the item has been loaded. If the data has not been loaded from a file, a file save dialog opens and user specifies a file. */ bool Item::overwrite(bool forceOverwrite, const std::string& formatId) { return ItemManager::overwrite(this, forceOverwrite, formatId); } void Item::updateLastAccessInformation(const std::string& filename, const std::string& formatId) { lastAccessedFileName_ = filename; lastAccessedFileFormatId_ = formatId; timeStampOfLastFileWriting_ = filesystem::last_write_time(filesystem::path(filename)); isConsistentWithLastAccessedFile_ = true; } /** Use this function to disable the implicit overwrite next time */ void Item::clearLastAccessInformation() { lastAccessedFileName_.clear(); lastAccessedFileFormatId_.clear(); isConsistentWithLastAccessedFile_ = false; } const Referenced* Item::customData(int id) const { if(id >= (int)extraData.size()){ return 0; } return extraData[id].get(); } Referenced* Item::customData(int id) { if(id >= (int)extraData.size()){ return 0; } return extraData[id].get(); } void Item::setCustomData(int id, ReferencedPtr data) { if(id >= (int)extraData.size()){ extraData.resize(id + 1, 0); } extraData[id] = data; } void Item::clearCustomData(int id) { if(customData(id)){ extraData[id] = 0; } } namespace { bool onNamePropertyChanged(Item* item, const string& name) { if(!name.empty()){ item->setName(name); } return !name.empty(); } } /** @if jp プロパティを表ã™ã‚­ãƒ¼ã¨å€¤ã®ãƒšã‚¢ã‚’出力ã™ã‚‹ã€‚ 通常ã€ãƒ—ロパティビューã«ã‚ˆã£ã¦å‘¼ã°ã‚Œã€å‡ºåŠ›çµæžœãŒãƒ—ロパティビューã«ãƒªã‚¹ãƒˆè¡¨ç¤ºã•ã‚Œã‚‹ã€‚ @param viewer ã“ã®ã‚ªãƒ–ジェクトã«å¯¾ã—ã¦ãƒ—ロパティを出力ã™ã‚‹ã€‚ @endif */ void Item::putProperties(PutPropertyFunction& putProperty) { putProperty(_("Name"), name_, bind(onNamePropertyChanged, this, _1)); std::string moduleName, className; ItemManager::getClassIdentifier(this, moduleName, className); putProperty(_("Class"), className); doPutProperties(putProperty); if(!lastAccessedFileName_.empty()){ putProperty(_("File"), lastAccessedFileName_); } putProperty(_("Refs"), refCounter()); putProperty(_("Sub item ?"), isSubItem_); } /** @if jp 派生クラスã«ã¦æœ¬é–¢æ•°ã‚’オーãƒãƒ©ã‚¤ãƒ‰ã™ã‚‹ã“ã¨ã§ã€ プロパティリストã®è¡¨ç¤ºã«å¯¾å¿œã•ã›ã‚‹ã“ã¨ãŒã§ãる。 プロパティã®å‡ºåŠ›ã«ã¯ Item::putProperty() 関数を使用ã™ã‚‹ã€‚ @note Item継承クラスãŒè¦ªã‚¯ãƒ©ã‚¹ï¼ˆåŸºåº•ã‚¯ãƒ©ã‚¹ï¼‰ã¨ã—ã¦å­˜åœ¨ã™ã‚‹å ´åˆã¯ã€é€šå¸¸ã€ 本関数ã®ä¸­ã§ã¾ãšè¦ªã‚¯ãƒ©ã‚¹ã® doPutProperties を呼ã³å‡ºã—〠ãã®å¾Œè¦ªã‚¯ãƒ©ã‚¹ã«å¯¾ã—ã¦è¿½åŠ ã•ã‚ŒãŸãƒ—ロパティを出力ã™ã‚‹ã€‚ @endif */ void Item::doPutProperties(PutPropertyFunction& putProperty) { } bool Item::store(Archive& archive) { return false; } bool Item::restore(const Archive& archive) { return false; } choreonoid-1.1.0+dfsg/src/Base/Item.h000066400000000000000000000174451207742442300173110ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_ITEM_H_INCLUDED #define CNOID_GUIBASE_ITEM_H_INCLUDED #include "SignalProxy.h" #include "ItemList.h" #include "PutPropertyFunction.h" #include #include #include #include #include "exportdecl.h" namespace cnoid { class Item; typedef boost::intrusive_ptr ItemPtr; class RootItem; class ItemTreeArchiver; class ExtensionManager; class PutPropertyFunction; class Archive; /** @if not jp @endif @if jp フレームワーク上ã§å…±æœ‰ã•ã‚Œã‚‹ã‚ªãƒ–ジェクトを表ã™ã‚¯ãƒ©ã‚¹ã€‚ モデル・ビュー・コントローラフレームワークã«ãŠã‘るモデル部分ã®æ ¸ã¨ãªã‚‹ã€‚ @endif */ class CNOID_EXPORT Item : public Referenced { public: enum Attribute { VISIBLE = 0, MOVABLE, NAME_EDITABLE, CHECKABLE, TREE_EXPANDED_BY_DEFAULT, NUM_ATTRIBUTES }; Item(); Item(const std::string& name); Item(const Item& item); inline const std::string& name() const { return name_; } virtual void setName(const std::string& name); inline bool hasAttribute(Attribute attribute) const { return attributes[attribute]; } inline Item* childItem() const { return firstChild_.get(); } inline Item* prevItem() const { return prevItem_; } inline Item* nextItem() const { return nextItem_.get(); } inline Item* parentItem() const { return parent_; } void addChildItem(ItemPtr item); void addSubItem(ItemPtr item); bool isSubItem() const; void detachFromParentItem(); void emitSigDetachedFromRootForSubTree(); void insertChildItem(ItemPtr item, ItemPtr nextItem); RootItem* findRootItem() const; Item* findItem(const std::string& path) const; template inline ItemType* findItem(const std::string& path) const { return dynamic_cast(findItem(path)); } template inline ItemList getSubItems() const { ItemList itemList; pickSubItems(childItem(), itemList); return itemList; } Item* headItem() const; template ItemType* findOwnerItem(bool includeSelf = false) { Item* parentItem__ = includeSelf ? this : parentItem(); while(parentItem__){ ItemType* ownerItem = dynamic_cast(parentItem__); if(ownerItem){ return ownerItem; } parentItem__ = parentItem__->parentItem(); } return 0; } void traverse(boost::function function); ItemPtr duplicate() const; ItemPtr duplicateAll() const; bool load(const std::string& filename, const std::string& formatId = std::string()); bool load(const std::string& filename, Item* parent, const std::string& formatId = std::string()); bool reload(); bool save(const std::string& filename, const std::string& formatId = std::string()); bool overwrite(bool forceOverwrite = false, const std::string& formatId = std::string()); void clearLastAccessInformation(); const std::string& lastAccessedFileName() const { return lastAccessedFileName_; } const std::string& lastAccessedFileFormatId() const { return lastAccessedFileFormatId_; } std::time_t timeStampOfLastFileWriting() const { return timeStampOfLastFileWriting_; } void setInconsistencyWithLastAccessedFile() { isConsistentWithLastAccessedFile_ = false; } bool isConsistentWithLastAccessedFile() const { return isConsistentWithLastAccessedFile_; } void putProperties(PutPropertyFunction& putProperty); virtual void notifyUpdate(); inline SignalProxy< boost::signal > sigNameChanged() { return sigNameChanged_; } inline SignalProxy< boost::signal > sigUpdated() { return sigUpdated_; } /** @if jp アイテムツリーã«ãŠã‘る本アイテムã®ä½ç½®ãŒå¤‰ã‚ã£ãŸã¨ã（新è¦ã«ãƒ„リーã«è¿½åŠ ã•ã‚ŒãŸã‚Šã€ ツリー内もã—ãã¯ãƒ„リー間ã§ç§»å‹•ã•ã‚ŒãŸã¨ã）ã«ç™ºè¡Œã•ã‚Œã‚‹ã‚·ã‚°ãƒŠãƒ«ï¼Ž ä½ç½®ãŒå¤‰åŒ–ã—ãŸã‹å¦ã‹ã¯ãƒ«ãƒ¼ãƒˆã‹ã‚‰ã®çµ¶å¯¾ä½ç½®ã«ã‚ˆã£ã¦åˆ¤å®šã™ã‚‹ãŸã‚〠親（祖先）アイテムã®ä½ç½®ãŒå¤‰ã‚ã‚Œã°å­ï¼ˆå­å­«ï¼‰ã‚¢ã‚¤ãƒ†ãƒ ã«å¯¾ã—ã¦ã‚‚本関数ãŒå‘¼ã°ã‚Œã‚‹ã€‚ 本シグナル㯠RootItem::sigTreeChanged() よりå‰ã«å‘¼ã°ã‚Œã‚‹ï¼Ž @endif */ inline SignalProxy< boost::signal > sigPositionChanged() { return sigPositionChanged_; } /** @note obsolete. */ inline SignalProxy< boost::signal > sigDetachedFromRoot() { return sigDetachedFromRoot_; } /** @note Please use this instead of sigDetachedFromRoot() */ inline SignalProxy< boost::signal > sigDisconnectedFromRoot() { return sigDetachedFromRoot_; } inline static SignalProxy< boost::signal > sigClassUnregistered() { return sigClassUnregistered_; } Referenced* customData(int id); const Referenced* customData(int id) const; void setCustomData(int id, ReferencedPtr data); void clearCustomData(int id); protected: // The destructor should not be called in usual ways virtual ~Item(); virtual void onConnectedToRoot(); virtual void onDisconnectedFromRoot(); virtual void onPositionChanged(); virtual ItemPtr doDuplicate() const; virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); virtual void doPutProperties(PutPropertyFunction& putProperty); static void pickSubItems(Item* item, ItemListBase& itemList); // void setAttribute(Attribute attribute) { attributes.set(attribute); } inline void unsetAttribute(Attribute attribute) { attributes.reset(attribute); } private: Item* parent_; ItemPtr firstChild_; Item* lastChild_; Item* prevItem_; ItemPtr nextItem_; std::string name_; std::bitset attributes; std::vector extraStates; std::vector extraData; bool isSubItem_; boost::signal sigNameChanged_; boost::signal sigDetachedFromRoot_; boost::signal sigUpdated_; boost::signal sigPositionChanged_; static boost::signal sigClassUnregistered_; // for file overwriting management, mainly accessed by ItemManagerImpl bool isConsistentWithLastAccessedFile_; std::string lastAccessedFileName_; std::string lastAccessedFileFormatId_; std::time_t timeStampOfLastFileWriting_; void init(); void doInsertChildItem(Item* item, Item* nextItem); void callSlotsOnPositionChanged(); void callFuncOnConnectedToRoot(); void detachFromParentItemSub(bool isMoving); void traverse(Item* item, const boost::function& function); ItemPtr duplicateAllSub(ItemPtr duplicated) const; void updateLastAccessInformation(const std::string& filename, const std::string& formatId); friend class RootItem; friend class ItemTreeArchiver; friend class ItemManagerImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ItemList.h000066400000000000000000000125341207742442300201370ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_ITEM_LIST_H_INCLUDED #define CNOID_GUIBASE_ITEM_LIST_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class Item; typedef boost::intrusive_ptr ItemPtr; template class ItemList; /** @if jp アイテムコンテナã«ãŠã„ã¦ã€ 自身ã®å¯¾è±¡ã¨ã™ã‚‹åž‹ã®ã‚¢ã‚¤ãƒ†ãƒ ã®ã¿ã‚’追加ã™ã‚‹ãŸã‚ã®ã‚¤ãƒ³ã‚¿ãƒ•ã‚§ãƒ¼ã‚¹ã¨ãªã‚‹æŠ½è±¡ã‚¯ãƒ©ã‚¹ã€‚ @endif */ class CNOID_EXPORT ItemListBase { public: ItemListBase() { } /** @if jp 代入演算å­ã€‚ コピー元リストãŒä¿æŒã™ã‚‹ã‚¢ã‚¤ãƒ†ãƒ ã®ã†ã¡ã€ コピー先リスã¨ã®åž‹ã«é©åˆã™ã‚‹ã‚‚ã®ã ã‘ãŒã‚³ãƒ”ーã•ã‚Œã‚‹ã€‚ @endif */ template inline ItemListBase& operator=(const ItemList& org) { clear(); for(size_t i=0; i < org.size(); ++i){ this->appendIfTypeMatches(org[i]); } return *this; } virtual ~ItemListBase() { } /** @if jp アイテムを追加ã™ã‚‹ã€‚ @param item 追加ã™ã‚‹ã‚¢ã‚¤ãƒ†ãƒ ã€‚ ã“ã®ã‚¢ã‚¤ãƒ†ãƒ ã®åž‹ãŒItemListBaseãŒå¯¾è±¡ã¨ã—ã¦ã„ã‚‹åž‹ã¨ä¸€è‡´ã—ã¦ã„ã‚‹ã¨ãã®ã¿ã€ 実際ã«è¿½åŠ ãŒè¡Œã‚れる。 @endif */ virtual bool appendIfTypeMatches(ItemPtr item) = 0; virtual void clear() = 0; }; /** @if jp 対象ã¨ã™ã‚‹åž‹ã®ã‚¢ã‚¤ãƒ†ãƒ ã®ã¿ã‚’ä¿æŒã™ã‚‹ã‚¢ã‚¤ãƒ†ãƒ ãƒªã‚¹ãƒˆã€‚ @endif */ template class ItemList : public ItemListBase { typedef boost::intrusive_ptr ItemTypePtr; typedef std::deque Container; public: typedef typename Container::iterator iterator; ItemList() { } /** @if jp コピーコンストラクタ。 コピー元リストãŒä¿æŒã™ã‚‹ã‚¢ã‚¤ãƒ†ãƒ ã®ã†ã¡ã€ コピー先リスã¨ã®åž‹ã«é©åˆã™ã‚‹ã‚‚ã®ã ã‘ãŒã‚³ãƒ”ーã•ã‚Œã‚‹ã€‚ @endif */ template ItemList(const ItemList& org){ for(size_t i=0; i < org.size(); ++i){ this->appendIfTypeMatches(org[i]); } } ItemList(const ItemList& org) : items(org.items) { } /** @if jp 代入演算å­ã€‚ コピー元リストãŒä¿æŒã™ã‚‹ã‚¢ã‚¤ãƒ†ãƒ ã®ã†ã¡ã€ コピー先リスã¨ã®åž‹ã«é©åˆã™ã‚‹ã‚‚ã®ã ã‘ãŒã‚³ãƒ”ーã•ã‚Œã‚‹ã€‚ @endif */ template inline ItemList& operator=(const ItemList& org){ static_cast(*this) = org; return *this; } inline ItemList& operator=(const ItemList& org){ items = org.items; return *this; } virtual ~ItemList() { } inline bool operator==(const ItemList& lhs){ return (items == lhs.items); } inline bool operator!=(const ItemList& lhs){ return (items != lhs.items); } inline bool empty() const { return items.empty(); } inline size_t size() const { return items.size(); } inline iterator begin() { return items.begin(); } inline iterator end() { return items.end(); } inline ItemType* back() const { return items.back().get(); } inline ItemType* front() const { return items.front().get(); } inline ItemType* operator[](size_t i) const { return items[i].get(); } inline void clear(){ items.clear(); } inline void append(ItemTypePtr item){ items.push_back(item); } inline void push_front(ItemTypePtr item){ items.push_front(item); } inline void push_back(ItemTypePtr item){ items.push_back(item); } inline void pop_front(){ items.pop_front(); } inline void pop_back(){ items.pop_back(); } virtual bool appendIfTypeMatches(ItemPtr item){ ItemTypePtr castedItem = boost::dynamic_pointer_cast(item); if(castedItem){ items.push_back(castedItem); } return castedItem; } inline boost::intrusive_ptr toSingle(bool allowFromMultiItems = false) const { return (items.size() == 1 || (allowFromMultiItems && !items.empty())) ? items[0] : ItemTypePtr(); } template inline boost::intrusive_ptr extractSingle(bool allowFromMultiItems = false) const { const ItemList narrowdItems(*this); return (narrowdItems.size() == 1 || (allowFromMultiItems && !narrowdItems.empty())) ? narrowdItems[0] : boost::intrusive_ptr(); } protected: Container items; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ItemManager.cpp000066400000000000000000001020251207742442300211240ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "ItemManagerImpl.h" #include "App.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" namespace { class DefaultCreationPanel : public ItemCreationPanel { QLineEdit* nameEntry; public: DefaultCreationPanel(QWidget* parent) : ItemCreationPanel(parent) { QHBoxLayout* layout = new QHBoxLayout(); layout->addWidget(new QLabel(_("Name:"))); nameEntry = new QLineEdit(); layout->addWidget(nameEntry); setLayout(layout); } virtual bool initializePanel(Item* protoItem) { nameEntry->setText(protoItem->name().c_str()); return true; } virtual bool initializeItem(Item* protoItem) { protoItem->setName(nameEntry->text().toStdString()); return true; } }; MessageView* messageView = 0; bool isStaticMembersInitialized = false; typedef map ClassInfoMap; ClassInfoMap typeIdToClassInfoMap; typedef map ClassInfoMapMap; ClassInfoMapMap moduleNameToClassNameToClassInfoMap; typedef map CreationPanelBaseMap; CreationPanelBaseMap creationPanelBaseMap; typedef map LoaderMap; LoaderMap extToLoaderMap; typedef list LoaderList; typedef map LoaderListMap; LoaderListMap typeIdToLoaderListMap; typedef map SaverListMap; static SaverListMap typeIdToSaverListMap; } ItemManager::ItemManager(const std::string& moduleName, MenuManager& menuManager) { impl = new ItemManagerImpl(moduleName, menuManager); } ItemManagerImpl::ItemManagerImpl(const string& moduleName, MenuManager& menuManager) : moduleName(moduleName), menuManager(menuManager) { if(!isStaticMembersInitialized){ menuManager.setPath("/File").setPath(N_("New ...")); menuManager.setPath("/File"); menuManager.addItem(_("Open Item")) ->sigTriggered().connect(bind(&ItemManagerImpl::onLoadItemActivated, this)); menuManager.setPath(N_("Open ...")); menuManager.setPath("/File"); menuManager.addItem(_("Reload Selected Items")) ->sigTriggered().connect(bind(&ItemManagerImpl::onReloadSelectedItemsActivated, this)); menuManager.addSeparator(); menuManager.addItem(_("Save Selected Items")) ->sigTriggered().connect(bind(&ItemManagerImpl::onSaveSelectedItemsActivated, this)); menuManager.addItem(_("Save Selected Items As")) ->sigTriggered().connect(bind(&ItemManagerImpl::onSaveSelectedItemsAsActivated, this)); menuManager.addItem(_("Save All Items")) ->sigTriggered().connect(bind(&ItemManagerImpl::onSaveAllItemsActivated, this)); menuManager.addSeparator(); menuManager.setPath(N_("Import ...")); menuManager.setPath("/File"); menuManager.addItem(_("Export Selected Items")) ->sigTriggered().connect(bind(&ItemManagerImpl::onExportSelectedItemsActivated, this)); menuManager.addSeparator(); messageView = MessageView::mainInstance(); isStaticMembersInitialized = true; } } ItemManager::~ItemManager() { delete impl; } ItemManagerImpl::~ItemManagerImpl() { // unregister creation panels for(set::iterator it = registeredCreationPanels.begin(); it != registeredCreationPanels.end(); ++it){ ItemCreationPanel* panel = *it; delete panel; } // unregister loaders for(set::iterator it = registeredLoaders.begin(); it != registeredLoaders.end(); ++it){ LoaderPtr loader = *it; for(size_t i=0; i < loader->extensions.size(); ++i){ const string& ext = loader->extensions[i]; LoaderMap::iterator p = extToLoaderMap.find(ext); if(p != extToLoaderMap.end()){ if(p->second == loader){ extToLoaderMap.erase(ext); } } } LoaderList& loaders = typeIdToLoaderListMap[loader->typeId]; LoaderList::iterator p = loaders.begin(); while(p != loaders.end()){ if(loader == *p){ p = loaders.erase(p); } else { p++; } } } // unregister savers for(set::iterator it = registeredSavers.begin(); it != registeredSavers.end(); ++it){ SaverPtr saver = *it; SaverList& savers = typeIdToSaverListMap[saver->typeId]; SaverList::iterator p = savers.begin(); while(p != savers.end()){ if(saver == *p){ p = savers.erase(p); } else { p++; } } } // unregister item class identifiers, CreationPanelBases and savers for(set::iterator q = registeredTypeIds.begin(); q != registeredTypeIds.end(); ++q){ const string& id = *q; Item::sigClassUnregistered_(id.c_str()); typeIdToClassInfoMap[id].reset(); CreationPanelBaseMap::iterator s = creationPanelBaseMap.find(id); if(s != creationPanelBaseMap.end()){ CreationPanelBase* base = s->second; delete base; creationPanelBaseMap.erase(s); } typeIdToLoaderListMap[id].clear(); typeIdToSaverListMap[id].clear(); } // unregister creation panel filters for(CreationPanelFilterSet::iterator p = registeredCreationPanelFilters.begin(); p != registeredCreationPanelFilters.end(); ++p){ ClassInfoPtr classInfo = typeIdToClassInfoMap[p->first]; if(classInfo){ classInfo->creationPanelBase->preFilters.remove(p->second); classInfo->creationPanelBase->postFilters.remove(p->second); } } moduleNameToClassNameToClassInfoMap.erase(moduleName); } void ItemManager::bindTextDomain(const std::string& domain) { impl->textDomain = domain; } void ItemManager::registerClassSub(FactoryBase* factory, const std::string& typeId, const std::string& className) { impl->registerClass(factory, typeId, className); } void ItemManagerImpl::registerClass(ItemManager::FactoryBase* factory, const string& typeId, const string& className) { ClassInfoPtr info(new ClassInfo()); info->moduleName = moduleName; info->className = className; info->factory = factory; typeIdToClassInfoMap[typeId] = info; moduleNameToClassNameToClassInfoMap[moduleName][className] = info; registeredTypeIds.insert(typeId); } bool ItemManager::getClassIdentifier(ItemPtr item, std::string& out_moduleName, std::string& out_className) { return ItemManagerImpl::getClassIdentifier(item, out_moduleName, out_className); } bool ItemManagerImpl::getClassIdentifier(ItemPtr item, string& out_moduleName, string& out_className) { bool result; ClassInfoMap::iterator p = typeIdToClassInfoMap.find(typeid(*item).name()); if(p != typeIdToClassInfoMap.end()){ ClassInfoPtr info = p->second; out_moduleName = info->moduleName; out_className = info->className; result = true; } else { out_moduleName.clear(); out_className = typeid(*item).name(); result = false; } return result; } ItemPtr ItemManager::create(const std::string& moduleName, const std::string& className) { return ItemManagerImpl::create(moduleName, className); } ItemPtr ItemManagerImpl::create(const string& moduleName, const string& className) { ItemPtr item; ClassInfoMapMap::iterator p = moduleNameToClassNameToClassInfoMap.find(moduleName); if(p != moduleNameToClassNameToClassInfoMap.end()){ ClassInfoMap& classNameToClassInfoMap = p->second; ClassInfoMap::iterator q = classNameToClassInfoMap.find(className); if(q != classNameToClassInfoMap.end()){ ClassInfoPtr info = q->second; ItemManager::FactoryBase* factory = info->factory; if(factory){ item = factory->create(); } } } return item; } void ItemManager::addCreationPanelSub(const std::string& typeId, ItemCreationPanel* panel) { impl->addCreationPanel(typeId, panel); } void ItemManagerImpl::addCreationPanel(const std::string& typeId, ItemCreationPanel* panel) { CreationPanelBase* base = getOrCreateCreationPanelBase(typeId); if(panel){ base->addPanel(panel); } else { base->addPanel(new DefaultCreationPanel(base)); } registeredCreationPanels.insert(panel); } void ItemManager::addCreationPanelFilterSub (const std::string& typeId, CreationPanelFilterBasePtr filter, bool afterInitializionByPanels) { impl->addCreationPanelFilter(typeId, filter, afterInitializionByPanels); } void ItemManagerImpl::addCreationPanelFilter (const std::string& typeId, ItemManager::CreationPanelFilterBasePtr filter, bool afterInitializionByPanels) { CreationPanelBase* base = getOrCreateCreationPanelBase(typeId); if(!afterInitializionByPanels){ base->preFilters.push_back(filter); } else { base->postFilters.push_back(filter); } registeredCreationPanelFilters.insert(make_pair(typeId, filter)); } ItemManagerImpl::CreationPanelBase* ItemManagerImpl::getOrCreateCreationPanelBase(const string& typeId) { CreationPanelBase* base = 0; ClassInfoMap::iterator p = typeIdToClassInfoMap.find(typeId); if(p != typeIdToClassInfoMap.end()){ ClassInfoPtr info = p->second; base = info->creationPanelBase; if(!base){ const char* untranslated = info->className.c_str(); const char* translated = dgettext(textDomain.c_str(), untranslated); bool isTranslated = (translated != untranslated); QString translatedClassName(translated); // set the class name without the "Item" suffix QString defaultName(translatedClassName); const char* suffix = isTranslated ? _("Item$") : "Item$"; defaultName.replace(QRegExp(suffix), ""); QString title(QString(_("Create New %1")).arg(translatedClassName)); ItemPtr protoItem = info->factory->create(); protoItem->setName(defaultName.toStdString()); base = new CreationPanelBase(title, protoItem); base->hide(); menuManager.setPath("/File/New ...").addItem(defaultName) ->sigTriggered().connect(bind(ItemManagerImpl::onNewItemActivated, base)); info->creationPanelBase = base; } } return base; } void ItemManagerImpl::onNewItemActivated(CreationPanelBase* base) { ItemTreeView* itemTreeView = ItemTreeView::mainInstance(); ItemList parentItems = itemTreeView->selectedItems(); if(parentItems.empty()){ parentItems.push_back(itemTreeView->rootItem()); } for(size_t i=0; i < parentItems.size(); ++i){ ItemPtr parentItem = parentItems[i]; ItemPtr newItem = base->createItem(parentItem); if(newItem){ parentItem->addChildItem(newItem); } } } ItemManagerImpl::CreationPanelBase::CreationPanelBase(const QString& title, ItemPtr protoItem) : QDialog(MainWindow::instance()), protoItem(protoItem) { setWindowTitle(title); QPushButton* createButton = new QPushButton(_("&Create")); createButton->setDefault(true); QPushButton* cancelButton = new QPushButton(_("&Cancel")); QDialogButtonBox* buttonBox = new QDialogButtonBox(this); buttonBox->addButton(createButton, QDialogButtonBox::AcceptRole); buttonBox->addButton(cancelButton, QDialogButtonBox::RejectRole); connect(buttonBox,SIGNAL(accepted()), this, SLOT(accept())); connect(buttonBox,SIGNAL(rejected()), this, SLOT(reject())); QVBoxLayout* topLayout = new QVBoxLayout(); panelLayout = new QVBoxLayout(); topLayout->addLayout(panelLayout); topLayout->addWidget(buttonBox); setLayout(topLayout); } void ItemManagerImpl::CreationPanelBase::addPanel(ItemCreationPanel* panel) { panelLayout->addWidget(panel); } ItemPtr ItemManagerImpl::CreationPanelBase::createItem(ItemPtr parentItem) { vector panels; int n = panelLayout->count(); for(int i=0; i < n; ++i){ ItemCreationPanel* panel = dynamic_cast(panelLayout->itemAt(i)->widget()); if(panel){ panels.push_back(panel); } } bool result = true; for(CreationPanelFilterList::iterator p = preFilters.begin(); p != preFilters.end(); ++p){ ItemManager::CreationPanelFilterBasePtr filter = *p; if(!(*filter)(protoItem.get(), parentItem.get())){ result = false; break; } } if(result){ for(size_t i=0; i < panels.size(); ++i){ if(!panels[i]->initializePanel(protoItem.get())){ result = false; break; } } } if(result){ if(exec() == QDialog::Accepted){ for(size_t i=0; i < panels.size(); ++i){ if(!panels[i]->initializeItem(protoItem.get())){ result = false; break; } } if(result){ for(CreationPanelFilterList::iterator p = postFilters.begin(); p != postFilters.end(); ++p){ ItemManager::CreationPanelFilterBasePtr filter = *p; if(!(*filter)(protoItem.get(), parentItem.get())){ result = false; break; } } } } else { result = false; } } ItemPtr newItem; if(result){ newItem = protoItem->duplicate(); } return newItem; } ItemCreationPanel* ItemCreationPanel::findPanelOnTheSameDialog(const std::string& name) { QBoxLayout* layout = dynamic_cast(parentWidget()); if(layout){ int n = layout->count(); for(int i=0; i < n; ++i){ ItemCreationPanel* panel = dynamic_cast(layout->itemAt(i)->widget()); if(panel){ if(panel->objectName().toStdString() == name){ return panel; } } } } return 0; } void ItemManagerImpl::expandExtensionsToVector(const string& extensions, vector& out_extensions) { typedef tokenizer< char_separator > tokenizer; char_separator sep(";"); tokenizer tokens(extensions, sep); for(tokenizer::iterator it = tokens.begin(); it != tokens.end(); ++it){ out_extensions.push_back(*it); } } void ItemManager::addLoaderSub (const std::string& typeId, const std::string& caption, const std::string& formatId, const std::string& extensions, FileFunctionBasePtr function, int priority) { impl->addLoader(typeId, caption, formatId, extensions, function, priority); } void ItemManagerImpl::addLoader (const std::string& typeId, const std::string& caption, const std::string& formatId, const std::string& extensions, ItemManager::FileFunctionBasePtr function, int priority) { ClassInfoMap::iterator p = typeIdToClassInfoMap.find(typeId); if(p != typeIdToClassInfoMap.end()){ LoaderPtr loader(new Loader()); loader->typeId = typeId; loader->caption = caption; loader->formatId = formatId; loader->priority = priority; loader->loadingFunction = function; loader->factory = p->second->factory; expandExtensionsToVector(extensions, loader->extensions); for(size_t i=0; i < loader->extensions.size(); ++i){ const string& ext = loader->extensions[i]; extToLoaderMap[ext] = loader; } bool isImporter = (priority <= ItemManager::PRIORITY_CONVERSION); if(!isImporter){ menuManager.setPath("/File/Open ..."); } else { menuManager.setPath("/File/Import ..."); } menuManager.addItem(caption.c_str()) ->sigTriggered().connect( bind(&ItemManagerImpl::onLoadSpecificTypeItemActivated, loader)); registeredLoaders.insert(loader); // insert loader to a proper position of the list considering priorities LoaderList& loaders = typeIdToLoaderListMap[typeId]; LoaderList::iterator it = loaders.begin(); while(true){ if(it == loaders.end()){ loaders.push_back(loader); break; } LoaderPtr loader2 = *it; if(loader->priority > loader2->priority){ loaders.insert(it, loader); break; } ++it; } } } bool ItemManager::load(Item* item, const std::string& filename, Item* parentItem, const std::string& formatId) { return ItemManagerImpl::load(item, filename, parentItem, formatId); } bool ItemManagerImpl::load(Item* item, const std::string& filename, Item* parentItem, const std::string& formatId) { bool loaded = false; LoaderPtr targetLoader; const string& typeId = typeid(*item).name(); LoaderList& loaders = typeIdToLoaderListMap[typeId]; if(!formatId.empty()){ for(LoaderList::iterator p = loaders.begin(); p != loaders.end(); ++p){ LoaderPtr& loader = *p; if(loader->formatId == formatId){ targetLoader = loader; break; } } } else { filesystem::path fpath(filename); string extension = filesystem::extension(fpath); if(extension.size() >= 2){ string ext = extension.substr(1); // remove dot for(LoaderList::iterator p = loaders.begin(); p != loaders.end(); ++p){ LoaderPtr& loader = *p; for(size_t i=0; i < loader->extensions.size(); ++i){ if(ext == loader->extensions[i]){ targetLoader = loader; break; } } } } } if(!targetLoader){ string message; if(formatId.empty()){ message = str(format(_("\"%1%\" cannot be loaded because the file format is unknown.")) % filename); } else { message = str(format(_("\"%1%\" cannot be loaded because file format \"%2%\" is unknown.")) % filename % formatId); } messageView->putln(message); } else { if(load(targetLoader, item, filename, parentItem)){ loaded = true; } } return loaded; } bool ItemManagerImpl::load(LoaderPtr loader, Item* item, const std::string& filename_, Item* parentItem) { bool loaded = false; if(loader->loadingFunction){ string filename(toActualPathName(filename_)); format f(_("Loading %1% \"%2%\"")); messageView->notify(str(format(_("Loading %1% \"%2%\"")) % loader->caption % filename)); ostringstream os; if(!parentItem){ parentItem = RootItem::mainInstance(); } if((*loader->loadingFunction)(item, filename, os, parentItem)){ if(item->name().empty()){ item->setName(filesystem::basename(filesystem::path(filename))); } item->updateLastAccessInformation(filename, loader->formatId); loaded = true; } if(!os.str().empty()){ messageView->putln(os.str()); } messageView->put(loaded ? _(" -> ok!\n") : _(" -> failed.\n")); messageView->flush(); } return loaded; } void ItemManagerImpl::onLoadItemActivated() { cout << "onLoadItemActivated()" << endl; } void ItemManagerImpl::onLoadSpecificTypeItemActivated(LoaderPtr loader) { QFileDialog dialog(MainWindow::instance()); dialog.setWindowTitle(str(format(_("Load %1%")) % loader->caption).c_str()); dialog.setFileMode(QFileDialog::ExistingFiles); dialog.setViewMode(QFileDialog::List); dialog.setLabelText(QFileDialog::Accept, _("Open")); dialog.setLabelText(QFileDialog::Reject, _("Cancel")); dialog.setDirectory(AppConfig::archive()->get("currentFileDialogDirectory", App::shareDirectory()).c_str()); QStringList filters; if(!loader->extensions.empty()){ string filterText = loader->caption + " ("; for(size_t i=0; i < loader->extensions.size(); ++i){ if(i > 0){ filterText += " "; } string extension = string("*.") + loader->extensions[i]; filterText += extension; } filterText += ")"; filters << filterText.c_str(); } filters << _("Any files (*)"); dialog.setNameFilters(filters); if(dialog.exec()){ AppConfig::archive()->writePath( "currentFileDialogDirectory", dialog.directory().absolutePath().toStdString()); QStringList filenames = dialog.selectedFiles(); Item* parentItem = ItemTreeView::mainInstance()->selectedItem(); if(!parentItem){ parentItem = RootItem::mainInstance(); } for(int i=0; i < filenames.size(); ++i){ ItemPtr item = loader->factory->create(); if(load(loader, item.get(), filesystem::path(filenames[i].toStdString()).file_string(), parentItem)){ parentItem->addChildItem(item); } } } } void ItemManager::addSaverSub(const std::string& typeId, const std::string& caption, const std::string& formatId, const std::string& extensions, FileFunctionBasePtr function, int priority) { impl->addSaver(typeId, caption, formatId, extensions, function, priority); } void ItemManagerImpl::addSaver (const string& typeId, const string& caption, const string& formatId, const string& extensions, ItemManager::FileFunctionBasePtr function, int priority) { ClassInfoMap::iterator p = typeIdToClassInfoMap.find(typeId); if(p != typeIdToClassInfoMap.end()){ SaverPtr saver(new Saver()); saver->typeId = typeId; saver->formatId = formatId; saver->caption = caption; saver->priority = priority; saver->savingFunction = function; expandExtensionsToVector(extensions, saver->extensions); // insert saver to a proper position of the list considering priorities SaverList& savers = typeIdToSaverListMap[typeId]; SaverList::iterator it = savers.begin(); while(true){ if(it == savers.end()){ savers.push_back(saver); break; } SaverPtr saver2 = *it; if(saver->priority > saver2->priority){ savers.insert(it, saver); break; } ++it; } registeredSavers.insert(saver); } } bool ItemManager::save(Item* item, const std::string& filename, const std::string& formatId) { return ItemManagerImpl::save(item, false, false, filename, formatId); } bool ItemManagerImpl::save (Item* item, bool useDialogToGetFilename, bool doExport, std::string filename, const std::string& formatId) { ClassInfoMap::iterator p = typeIdToClassInfoMap.find(typeid(*item).name()); if(p == typeIdToClassInfoMap.end()){ return false; } ClassInfoPtr classInfo = p->second; bool saved = false; bool tryToSave = false; string itemLabel = classInfo->className + " \"" + item->name() + "\""; SaverList& savers = typeIdToSaverListMap[typeid(*item).name()]; SaverPtr targetSaver; if(useDialogToGetFilename){ targetSaver = getSaverAndFilenameFromSaveDialog(savers, doExport, itemLabel, formatId, filename); } else { targetSaver = determineSaver(savers, filename, formatId); } if(targetSaver && targetSaver->savingFunction){ tryToSave = true; if(!doExport){ messageView->put(str(format(_("Saving %1% to \"%2%\"")) % itemLabel % filename)); } else { messageView->put(str(format(_("Exporting %1% into \"%2%\"")) % itemLabel % filename)); } ostringstream os; Item* parentItem = item->parentItem(); if(!parentItem){ parentItem = RootItem::mainInstance(); } if((*targetSaver->savingFunction)(item, filename, os, parentItem)){ saved = true; bool isExporter = (targetSaver->priority <= ItemManager::PRIORITY_CONVERSION); if(!isExporter){ item->updateLastAccessInformation(filename, targetSaver->formatId); } } messageView->put(saved ? _(" -> ok!\n") : _(" -> failed.\n")); if(!os.str().empty()){ messageView->put(os.str() + "\n"); } } if(!tryToSave){ string actualFormatId = targetSaver ? targetSaver->formatId : formatId; if(actualFormatId.empty()){ if(!doExport){ messageView->put(str(format(_("%1% cannot be saved.\n")) % itemLabel)); } else { messageView->put(str(format(_("%1% cannot be exported.\n")) % itemLabel)); } } else { if(!doExport){ messageView->put(str(format(_("%1% cannot be saved as the %2% format.\n")) % itemLabel % actualFormatId)); } else { messageView->put(str(format(_("%1% cannot be exported into the %2% format.\n")) % itemLabel % actualFormatId)); } } } return saved; } ItemManagerImpl::SaverPtr ItemManagerImpl::getSaverAndFilenameFromSaveDialog (SaverList& savers, bool doExport, const string& itemLabel, const string& formatId, string& io_filename) { QFileDialog dialog(MainWindow::instance()); dialog.setWindowTitle(str(format(_("Save %1% as")) % itemLabel).c_str()); dialog.setFileMode(QFileDialog::AnyFile); dialog.setViewMode(QFileDialog::List); dialog.setLabelText(QFileDialog::Accept, _("Save")); dialog.setLabelText(QFileDialog::Reject, _("Cancel")); if(!io_filename.empty()){ dialog.selectFile(io_filename.c_str()); io_filename.clear(); } QStringList filters; vector activeSavers; for(SaverList::iterator p = savers.begin(); p != savers.end(); ++p){ SaverPtr& saver = *p; bool isExporter = (saver->priority <= ItemManager::PRIORITY_CONVERSION); if((doExport && !isExporter) || (!doExport && isExporter)){ continue; } if(!formatId.empty() && saver->formatId != formatId){ continue; } string filterText = saver->caption + " ("; if(saver->extensions.empty()){ filterText += "*"; } else { for(size_t i=0; i < saver->extensions.size(); ++i){ if(i > 0){ filterText += " "; } string extension = string("*.") + saver->extensions[i]; filterText += extension; } } filterText += ")"; filters << filterText.c_str(); activeSavers.push_back(saver); } dialog.setNameFilters(filters); SaverPtr targetSaver; if(filters.size() > 0){ dialog.setDirectory(AppConfig::archive()->get("currentFileDialogDirectory", App::shareDirectory()).c_str()); if(dialog.exec() == QFileDialog::Accepted){ AppConfig::archive()->writePath( "currentFileDialogDirectory", dialog.directory().absolutePath().toStdString()); io_filename = dialog.selectedFiles()[0].toStdString(); if(!io_filename.empty()){ int saverIndex = -1; QString selectedFilter = dialog.selectedNameFilter(); for(int i=0; i < filters.size(); ++i){ if(filters[i] == selectedFilter){ saverIndex = i; break; } } if(saverIndex >= 0){ targetSaver = activeSavers[saverIndex]; // add a lacking extension automatically if(!targetSaver->extensions.empty()){ string ext = filesystem::extension(filesystem::path(io_filename)); bool extLacking = true; if(!ext.empty()){ ext = ext.substr(1); // remove the first dot for(size_t i=0; i < targetSaver->extensions.size(); ++i){ if(ext == targetSaver->extensions[i]){ extLacking = false; break; } } } if(extLacking){ io_filename += "."; io_filename += targetSaver->extensions.front(); } } } } } } return targetSaver; } ItemManagerImpl::SaverPtr ItemManagerImpl::determineSaver (SaverList& savers, const string& filename, const string& formatId) { SaverPtr targetSaver; if(!formatId.empty()){ for(SaverList::iterator p = savers.begin(); p != savers.end(); ++p){ SaverPtr& saver = *p; if(saver->formatId == formatId){ targetSaver = saver; break; } } } else { string extension = filesystem::extension(filesystem::path(filename)); for(SaverList::iterator p = savers.begin(); p != savers.end(); ++p){ SaverPtr& saver = *p; for(size_t i=0; i < saver->extensions.size(); ++i){ if(saver->extensions[i] == extension){ targetSaver = saver; break; } } } if(!targetSaver && !savers.empty()){ targetSaver = savers.front(); } } return targetSaver; } bool ItemManager::overwrite(Item* item, bool forceOverwrite, const std::string& formatId) { return ItemManagerImpl::overwrite(item, forceOverwrite, formatId); } bool ItemManagerImpl::overwrite(Item* item, bool forceOverwrite, const std::string& formatId) { bool needToOverwrite = forceOverwrite; string filename(item->lastAccessedFileName()); string lastFormatId(item->lastAccessedFileFormatId()); string defaultFilenameOnDialog; if(filename.empty()){ defaultFilenameOnDialog = item->name(); } if(!formatId.empty() && formatId != lastFormatId){ needToOverwrite = true; } else { if(!filename.empty()){ filesystem::path fpath(filename); if(!filesystem::exists(fpath) || filesystem::last_write_time(fpath) > item->timeStampOfLastFileWriting()){ needToOverwrite = true; filename.clear(); } } } if(!needToOverwrite && !item->isConsistentWithLastAccessedFile()){ needToOverwrite = true; } bool synchronized = !needToOverwrite; if(!synchronized){ if(!filename.empty() && formatId.empty()){ synchronized = save(item, false, false, filename, lastFormatId); } if(!synchronized){ synchronized = save(item, true, false, defaultFilenameOnDialog, formatId); } } return synchronized; } void ItemManagerImpl::onReloadSelectedItemsActivated() { ItemList selectedItems = ItemTreeView::mainInstance()->selectedItems(); for(size_t i=0; i < selectedItems.size(); ++i){ string s, t; selectedItems[i]->reload(); } } void ItemManagerImpl::onSaveSelectedItemsActivated() { ItemList selectedItems = ItemTreeView::mainInstance()->selectedItems(); for(size_t i=0; i < selectedItems.size(); ++i){ overwrite(selectedItems[i], true, ""); } } void ItemManagerImpl::onSaveSelectedItemsAsActivated() { ItemList selectedItems = ItemTreeView::mainInstance()->selectedItems(); for(size_t i=0; i < selectedItems.size(); ++i){ string formatId; save(selectedItems[i], true, false, selectedItems[i]->headItem()->name(), formatId); } } void ItemManagerImpl::onSaveAllItemsActivated() { } void ItemManagerImpl::onExportSelectedItemsActivated() { ItemList selectedItems = ItemTreeView::mainInstance()->selectedItems(); for(size_t i=0; i < selectedItems.size(); ++i){ string formatId; save(selectedItems[i], true, true, selectedItems[i]->headItem()->name(), formatId); } } choreonoid-1.1.0+dfsg/src/Base/ItemManager.h000066400000000000000000000172751207742442300206050ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_ITEM_MANAGER_H_INCLUDED #define CNOID_GUIBASE_ITEM_MANAGER_H_INCLUDED #include "ExtensionManager.h" #include "ItemList.h" #include #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class MenuManager; class Item; typedef boost::intrusive_ptr ItemPtr; class ItemManagerImpl; class ItemCreationPanel : public QWidget { public: ItemCreationPanel(QWidget* parent = 0) : QWidget(parent) { } virtual bool initializePanel(Item* protoItem) = 0; virtual bool initializeItem(Item* protoItem) = 0; protected: ItemCreationPanel* findPanelOnTheSameDialog(const std::string& name); }; class CNOID_EXPORT ItemManager { public: ItemManager(const std::string& moduleName, MenuManager& menuManager); ~ItemManager(); private: class FactoryBase { public: virtual ItemPtr create() = 0; }; template class Factory : public FactoryBase { public: virtual ItemPtr create() { return new ItemType(); } }; class CreationPanelFilterBase { public: virtual ~CreationPanelFilterBase() { } virtual bool operator()(Item* protoItem, Item* parentItem) = 0; }; typedef boost::shared_ptr CreationPanelFilterBasePtr; class FileFunctionBase { public: virtual ~FileFunctionBase() { } virtual bool operator()(Item* item, const std::string& filename, std::ostream& os, Item* parentItem) = 0; }; typedef boost::shared_ptr FileFunctionBasePtr; class OverwritingCheckFunctionBase { public: ~OverwritingCheckFunctionBase(); virtual bool operator()(Item* item) = 0; }; typedef boost::shared_ptr OverwritingCheckFunctionBasePtr; void registerClassSub(FactoryBase* factory, const std::string& typeId, const std::string& className); void addCreationPanelSub(const std::string& typeId, ItemCreationPanel* panel); void addCreationPanelFilterSub( const std::string& typeId, CreationPanelFilterBasePtr filter, bool afterInitializionByPanels); void addLoaderSub(const std::string& typeId, const std::string& caption, const std::string& formatId, const std::string& extensions, FileFunctionBasePtr function, int priority); void addSaverSub(const std::string& typeId, const std::string& caption, const std::string& formatId, const std::string& extensions, FileFunctionBasePtr function, int priority); public: void bindTextDomain(const std::string& domain); enum { PRIORITY_CONVERSION = -10, PRIORITY_OPTIONAL = 0, PRIORITY_DEFAULT = 10, PRIORITY_FORCE = 20 }; template class CreationPanelFilter : public CreationPanelFilterBase { public: typedef boost::function Function; CreationPanelFilter(Function function) : function(function) { } virtual bool operator()(Item* protoItem, Item* parentItem){ return function(static_cast(protoItem), parentItem); } private: Function function; }; template class FileFunction : public FileFunctionBase { public: typedef boost::function Function; FileFunction(Function function) : function(function) { } virtual bool operator()(Item* item, const std::string& filename, std::ostream& os, Item* parentItem){ return function(static_cast(item), filename, os, parentItem); } private: Function function; }; template void registerClass(const std::string& className) { registerClassSub(new Factory(), typeid(ItemType).name(), className); } template void registerDerivedClass(const std::string& className) { // registerClassSub(new Factory(), typeid(ItemType).name(), className); } static bool getClassIdentifier(ItemPtr item, std::string& out_moduleName, std::string& out_className); static ItemPtr create(const std::string& moduleName, const std::string& itemClassName); template void addCreationPanel(ItemCreationPanel* panel = 0) { addCreationPanelSub(typeid(ItemType).name(), panel); } template void addCreationPanelPreFilter(const typename CreationPanelFilter::Function& filter) { addCreationPanelFilterSub(typeid(ItemType).name(), CreationPanelFilterBasePtr(new CreationPanelFilter(filter)), false); } template void addCreationPanelPostFilter(const typename CreationPanelFilter::Function& filter){ addCreationPanelFilterSub(typeid(ItemType).name(), CreationPanelFilterBasePtr(new CreationPanelFilter(filter)), true); } template void addLoader(const std::string& caption, const std::string& formatId, const std::string& extensions, const typename FileFunction::Function& function, int priority = PRIORITY_DEFAULT) { addLoaderSub(typeid(ItemType).name(), caption, formatId, extensions, FileFunctionBasePtr(new FileFunction(function)), priority); } template void addSaver(const std::string& caption, const std::string& formatId, const std::string& extensions, const typename FileFunction::Function& function, int priority = PRIORITY_DEFAULT){ addSaverSub(typeid(ItemType).name(), caption, formatId, extensions, FileFunctionBasePtr(new FileFunction(function)), priority); } template void addLoaderAndSaver(const std::string& caption, const std::string& formatId, const std::string& extensions, const typename FileFunction::Function& loadingFunction, const typename FileFunction::Function& savingFunction, int priority = PRIORITY_DEFAULT){ addLoader(caption, formatId, extensions, loadingFunction, priority); addSaver(caption, formatId, extensions, savingFunction, priority); } private: // The following static functions are called from functions in the Item class static bool load(Item* item, const std::string& filename, Item* parentItem, const std::string& formatId); static bool save(Item* item, const std::string& filename, const std::string& formatId); static bool overwrite(Item* item, bool forceOverwrite, const std::string& formatId); // overwrite friend class Item; friend class ItemManagerImpl; ItemManagerImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ItemManagerImpl.h000066400000000000000000000117141207742442300214170ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_ITEMMANAGERIMPL_H_INCLUDED #define CNOID_ITEMMANAGERIMPL_H_INCLUDED #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class ItemManagerImpl { public: ItemManagerImpl(const string& moduleName, MenuManager& menuManager); ~ItemManagerImpl(); typedef list CreationPanelFilterList; typedef set< pair > CreationPanelFilterSet; class CreationPanelBase : public QDialog { public: CreationPanelBase(const QString& title, ItemPtr protoItem); void addPanel(ItemCreationPanel* panel); ItemPtr createItem(ItemPtr parentItem); CreationPanelFilterList preFilters; CreationPanelFilterList postFilters; private: QVBoxLayout* panelLayout; ItemPtr protoItem; }; struct ClassInfo { ClassInfo() { factory = 0; creationPanelBase = 0; } ~ClassInfo() { delete factory; delete creationPanelBase; } string moduleName; string className; ItemManager::FactoryBase* factory; CreationPanelBase* creationPanelBase; }; typedef boost::shared_ptr ClassInfoPtr; class Loader : public QObject { public: string typeId; string formatId; string caption; int priority; ItemManager::FileFunctionBasePtr loadingFunction; ItemManager::FactoryBase* factory; vector extensions; }; typedef boost::shared_ptr LoaderPtr; struct Saver { string typeId; string formatId; string caption; int priority; vector extensions; ItemManager::FileFunctionBasePtr savingFunction; }; typedef boost::shared_ptr SaverPtr; typedef list SaverList; string moduleName; string textDomain; MenuManager& menuManager; set registeredTypeIds; set registeredCreationPanels; CreationPanelFilterSet registeredCreationPanelFilters; set registeredLoaders; set registeredSavers; QSignalMapper* mapperForNewItemActivated; QSignalMapper* mapperForLoadSpecificTypeItemActivated; void registerClass(ItemManager::FactoryBase* factory, const string& typeId, const string& className); static bool getClassIdentifier(ItemPtr item, string& out_moduleName, string& out_className); static ItemPtr create(const string& moduleName, const string& className); void addCreationPanel(const std::string& typeId, ItemCreationPanel* panel); void addCreationPanelFilter( const std::string& typeId, ItemManager::CreationPanelFilterBasePtr filter, bool afterInitializionByPanels); CreationPanelBase* getOrCreateCreationPanelBase(const string& typeId); static void expandExtensionsToVector(const string& extensions, vector& out_extensions); void addLoader (const std::string& typeId, const std::string& caption, const std::string& formatId, const std::string& extensions, const ItemManager::FileFunctionBasePtr function, int priority); static bool load(Item* item, const std::string& filename, Item* parentItem, const std::string& formatId); static bool load(LoaderPtr loader, Item* item, const std::string& filename, Item* parentItem); void addSaver (const string& typeId, const string& caption, const string& formatId, const string& extensions, ItemManager::FileFunctionBasePtr function, int priority); static bool save(Item* item, bool useDialogToGetFilename, bool doExport, std::string filename, const std::string& formatId); static SaverPtr getSaverAndFilenameFromSaveDialog( SaverList& savers, bool doExport, const string& itemLabel, const string& formatId, string& io_filename); static SaverPtr determineSaver(SaverList& savers, const string& filename, const string& formatId); static bool overwrite(Item* item, bool forceOverwrite, const std::string& formatId); static void onNewItemActivated(CreationPanelBase* base); void onLoadItemActivated(); static void onLoadSpecificTypeItemActivated(LoaderPtr loader); void onReloadSelectedItemsActivated(); void onSaveSelectedItemsActivated(); void onSaveSelectedItemsAsActivated(); void onSaveAllItemsActivated(); void onExportSelectedItemsActivated(); }; } #endif choreonoid-1.1.0+dfsg/src/Base/ItemPath.cpp000066400000000000000000000015521207742442300204510ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ItemPath.h" #include "Item.h" #include "RootItem.h" using namespace std; using namespace boost; using namespace cnoid; ItemPath::ItemPath(const std::string& path) : path(path) { Tokenizer pathElements(path, Separator("\\", "/", "")); pathBegin = pathElements.begin(); pathEnd = pathElements.end(); if(pathBegin != pathEnd && (*pathBegin).empty()){ pathBegin++; isAbsolute_ = true; } else { isAbsolute_ = false; } pathLeaf = pathBegin; for(iterator it = pathBegin; it != pathEnd; ++it){ pathLeaf = it; } } std::string ItemPath::folder() { std::string path; iterator it = pathBegin; while(true){ path.append(*it++); if(it == pathLeaf){ break; } path.append("/"); } return path; } choreonoid-1.1.0+dfsg/src/Base/ItemPath.h000066400000000000000000000021001207742442300201040ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_ITEM_PATH_H_INCLUDED #define CNOID_GUIBASE_ITEM_PATH_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT ItemPath { typedef boost::escaped_list_separator Separator; typedef boost::tokenizer Tokenizer; public: typedef Tokenizer::iterator iterator; typedef iterator Iterator; ItemPath(const std::string& path); inline bool isAbsolute() { return isAbsolute_; } inline bool isRelative() { return !isAbsolute_; } inline iterator begin() { return pathBegin; } inline iterator end() { return pathEnd; } std::string folder(); inline std::string leaf() { return *pathLeaf; } private: std::string path; Tokenizer::iterator pathBegin; Tokenizer::iterator pathLeaf; Tokenizer::iterator pathEnd; bool isAbsolute_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ItemPropertyView.cpp000066400000000000000000000236001207742442300222320ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ItemPropertyView.h" #include "ItemTreeView.h" #include "Item.h" #include "ItemList.h" #include "ItemManager.h" #include "MessageView.h" #include "PutPropertyFunction.h" #include "ConnectionSet.h" #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { bool TRACE_FUNCTIONS = false; typedef variant ValueVariant; typedef variant, function, function, function, function > FunctionVariant; enum TypeId { TYPE_BOOL, TYPE_INT, TYPE_DOUBLE, TYPE_STRING, TYPE_SELECTION }; struct Property { Property(const string& name, ValueVariant value) : name(name), value(value), hasValidFunction(false) { } Property(const string& name, ValueVariant value, FunctionVariant func) : name(name), value(value), func(func), hasValidFunction(true) { } string name; ValueVariant value; FunctionVariant func; bool hasValidFunction; }; typedef shared_ptr PropertyPtr; class PropertyItem : public QTableWidgetItem { public: PropertyItem(ItemPropertyViewImpl* viewImpl, ValueVariant value); PropertyItem(ItemPropertyViewImpl* viewImpl, ValueVariant value, FunctionVariant func); virtual QVariant data(int role) const; virtual void setData(int role, const QVariant& qvalue); ItemPropertyViewImpl* itemPropertyViewImpl; ValueVariant value; FunctionVariant func; bool hasValidFunction; }; class PropertyDelegate : public QItemDelegate { public: PropertyDelegate(QObject* parent); QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const; void setEditorData(QWidget* editor, const QModelIndex& index) const; void setModelData(QWidget* editor, QAbstractItemModel* model, const QModelIndex& index) const; void updateEditorGeometry(QWidget* editor, const QStyleOptionViewItem& option, const QModelIndex& index) const; }; } namespace cnoid { class ItemPropertyViewImpl : public PutPropertyFunction { public: ItemPropertyViewImpl(ItemPropertyView* self); ~ItemPropertyViewImpl(); void updateProperties(); void addProperty(const std::string& name, PropertyItem* propertyItem); void onItemSelectionChanged(const ItemList& items); ItemPropertyView* self; QTableWidget* tableWidget; ItemPtr currentItem; ConnectionSet itemConnections; int tmpListIndex; std::vector properties; PropertyPtr editedProperty; bool isPressedPathValid; // PutPropertyFunction's virtual functions virtual void operator()(const std::string& name, bool value){ addProperty(name, new PropertyItem(this, value)); } virtual void operator()(const std::string& name, bool value, const boost::function& func) { addProperty(name, new PropertyItem(this, value, func)); } virtual void operator()(const std::string& name, int value){ addProperty(name, new PropertyItem(this, value)); } virtual void operator()(const std::string& name, int value, const boost::function& func){ addProperty(name, new PropertyItem(this, value, func)); } virtual void operator()(const std::string& name, double value){ addProperty(name, new PropertyItem(this, value)); } virtual void operator()(const std::string& name, double value, const boost::function& func){ addProperty(name, new PropertyItem(this, value, func)); } virtual void operator()(const std::string& name, const std::string& value){ addProperty(name, new PropertyItem(this, value)); } virtual void operator()(const std::string& name, const std::string& value, const boost::function& func){ addProperty(name, new PropertyItem(this, value, func)); } void operator()(const std::string& name, Selection selection){ addProperty(name, new PropertyItem(this, selection)); } void operator()(const std::string& name, Selection selection, const boost::function& func){ addProperty(name, new PropertyItem(this, selection, func)); } }; } PropertyItem::PropertyItem(ItemPropertyViewImpl* viewImpl, ValueVariant value) : itemPropertyViewImpl(viewImpl), value(value) { setFlags(Qt::ItemIsEnabled); hasValidFunction = false; } PropertyItem::PropertyItem(ItemPropertyViewImpl* viewImpl, ValueVariant value, FunctionVariant func) : itemPropertyViewImpl(viewImpl), value(value), func(func) { setFlags(Qt::ItemIsEnabled|Qt::ItemIsEditable); hasValidFunction = true; } QVariant PropertyItem::data(int role) const { if(role == Qt::DisplayRole || role == Qt::EditRole){ switch(value.which()){ case TYPE_BOOL: return get(value); case TYPE_INT: return get(value); case TYPE_DOUBLE: return get(value); case TYPE_STRING: return get(value).c_str(); case TYPE_SELECTION: return get(value).label().c_str(); } } return QTableWidgetItem::data(role); } void PropertyItem::setData(int role, const QVariant& qvalue) { bool accepted = false; if(role == Qt::EditRole){ switch(qvalue.type()){ case QVariant::Bool: accepted = get< function >(func)(qvalue.toBool()); break; case QVariant::String: accepted = get< function >(func)(qvalue.toString().toStdString()); break; case QVariant::Int: accepted = get< function >(func)(qvalue.toInt()); break; case QVariant::Double: accepted = get< function >(func)(qvalue.toDouble()); break; default: break; } if(accepted){ itemPropertyViewImpl->currentItem->notifyUpdate(); } } } void ItemPropertyView::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ext->addView(new ItemPropertyView()); initialized = true; } } ItemPropertyView::ItemPropertyView() { impl = new ItemPropertyViewImpl(this); } ItemPropertyViewImpl::ItemPropertyViewImpl(ItemPropertyView* self) : self(self) { self->setName(N_("Property")); self->setDefaultLayoutArea(View::LEFT_BOTTOM); isPressedPathValid = false; tableWidget = new QTableWidget(self); tableWidget->setFrameShape(QFrame::NoFrame); tableWidget->setColumnCount(2); tableWidget->setSelectionBehavior(QAbstractItemView::SelectRows); tableWidget->setSelectionMode(QAbstractItemView::NoSelection); tableWidget->horizontalHeader()->hide(); tableWidget->horizontalHeader()->setStretchLastSection(true); tableWidget->verticalHeader()->hide(); tableWidget->verticalHeader()->setResizeMode(QHeaderView::ResizeToContents); QVBoxLayout* vbox = new QVBoxLayout(); vbox->addWidget(tableWidget); self->setLayout(vbox); ItemTreeView::mainInstance()->sigSelectionChanged().connect( bind(&ItemPropertyViewImpl::onItemSelectionChanged, this, _1)); } ItemPropertyView::~ItemPropertyView() { delete impl; } ItemPropertyViewImpl::~ItemPropertyViewImpl() { itemConnections.disconnect(); } void ItemPropertyViewImpl::updateProperties() { tableWidget->setRowCount(0); if(currentItem){ tmpListIndex = 0; properties.clear(); currentItem->putProperties(*this); } } void ItemPropertyViewImpl::addProperty(const std::string& name, PropertyItem* propertyItem) { int row = tableWidget->rowCount(); tableWidget->setRowCount(row + 1); QTableWidgetItem* nameItem = new QTableWidgetItem(name.c_str()); nameItem->setFlags(Qt::ItemIsEnabled); tableWidget->setItem(row, 0, nameItem); tableWidget->setItem(row, 1, propertyItem); } void ItemPropertyViewImpl::onItemSelectionChanged(const ItemList& items) { if(TRACE_FUNCTIONS){ cout << "ItemPropertyView::onItemSelectionChanged()" << endl; } ItemPtr item = items.toSingle(); if(item != currentItem){ currentItem = item; itemConnections.disconnect(); if(item){ itemConnections.add( item->sigUpdated().connect( bind(&ItemPropertyViewImpl::updateProperties, this))); itemConnections.add( item->sigNameChanged().connect( bind(&ItemPropertyViewImpl::updateProperties, this))); } updateProperties(); } } PropertyDelegate::PropertyDelegate(QObject* parent) : QItemDelegate(parent) { } QWidget* PropertyDelegate::createEditor (QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const { return 0; } void PropertyDelegate::setEditorData(QWidget* editor, const QModelIndex& index) const { } void PropertyDelegate::setModelData (QWidget* editor, QAbstractItemModel* model, const QModelIndex& index) const { } void PropertyDelegate::updateEditorGeometry (QWidget* editor, const QStyleOptionViewItem& option, const QModelIndex& index) const { editor->setGeometry(option.rect); } choreonoid-1.1.0+dfsg/src/Base/ItemPropertyView.h000066400000000000000000000007271207742442300217040ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_ITEM_PROPERTY_VIEW_H_INCLUDED #define CNOID_GUIBASE_ITEM_PROPERTY_VIEW_H_INCLUDED #include namespace cnoid { class ItemPropertyViewImpl; class ItemPropertyView : public View { public: static void initialize(ExtensionManager* ext); ItemPropertyView(); ~ItemPropertyView(); private: ItemPropertyViewImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ItemSelectionModel.cpp000066400000000000000000000032721207742442300224640ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ItemSelectionModel.h" using namespace cnoid; ItemSelectionModel::ItemSelectionModel(QAbstractItemModel* model) : QItemSelectionModel(model) { initialize(); } ItemSelectionModel::ItemSelectionModel(QAbstractItemModel* model, QObject* parent) : QItemSelectionModel(model, parent) { initialize(); } void ItemSelectionModel::initialize() { connect(this, SIGNAL(currentChanged(const QModelIndex&, const QModelIndex&)), this, SLOT(onCurrentChanged(const QModelIndex&, const QModelIndex&))); connect(this, SIGNAL(currentColumnChanged(const QModelIndex&, const QModelIndex&)), this, SLOT(onCurrentColumnChanged(const QModelIndex&, const QModelIndex&))); connect(this, SIGNAL(currentRowChanged(const QModelIndex&, const QModelIndex&)), this, SLOT(onCurrentRowChanged(const QModelIndex&, const QModelIndex&))); connect(this, SIGNAL(selectionChanged(const QItemSelection&, const QItemSelection&)), this, SLOT(onSelectionChanged(const QItemSelection&, const QItemSelection&))); } void ItemSelectionModel::onCurrentChanged(const QModelIndex& index, const QModelIndex& previous) { sigCurrentChanged_(index, previous); } void ItemSelectionModel::onCurrentColumnChanged(const QModelIndex& index, const QModelIndex& previous) { sigCurrentColumnChanged_(index, previous); } void ItemSelectionModel::onCurrentRowChanged(const QModelIndex& index, const QModelIndex& previous) { sigCurrentRowChanged_(index, previous); } void ItemSelectionModel::onSelectionChanged(const QItemSelection& selected, const QItemSelection& deselected) { sigSelectionChanged_(selected, deselected); } choreonoid-1.1.0+dfsg/src/Base/ItemSelectionModel.h000066400000000000000000000041001207742442300221200ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_ITEM_SELECTION_MODEL_H_INCLUDED #define CNOID_GUIBASE_ITEM_SELECTION_MODEL_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT ItemSelectionModel : public QItemSelectionModel { Q_OBJECT public: ItemSelectionModel(QAbstractItemModel* model); ItemSelectionModel(QAbstractItemModel* model, QObject* parent); inline SignalProxy< boost::signal > sigCurrentChanged() { return sigCurrentChanged_; } inline SignalProxy< boost::signal > sigCurrentColumnChanged() { return sigCurrentColumnChanged_; } inline SignalProxy< boost::signal > sigCurrentRowChanged() { return sigCurrentRowChanged_; } inline SignalProxy< boost::signal > sigSelectionChanged() { return sigSelectionChanged_; } private Q_SLOTS: void onCurrentChanged(const QModelIndex& index, const QModelIndex& previous); void onCurrentColumnChanged(const QModelIndex& index, const QModelIndex& previous); void onCurrentRowChanged(const QModelIndex& index, const QModelIndex& previous); void onSelectionChanged(const QItemSelection& selected, const QItemSelection& deselected); private: boost::signal sigCurrentChanged_; boost::signal sigCurrentColumnChanged_; boost::signal sigCurrentRowChanged_; boost::signal sigSelectionChanged_; void initialize(); }; } #endif choreonoid-1.1.0+dfsg/src/Base/ItemTreeArchiver.cpp000066400000000000000000000122361207742442300221410ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ItemTreeArchiver.h" #include "RootItem.h" #include "ItemManager.h" #include "PluginManager.h" #include "MessageView.h" #include "Archive.h" #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; ItemTreeArchiver::ItemTreeArchiver() { messageView = MessageView::mainInstance(); } ItemTreeArchiver::~ItemTreeArchiver() { } ArchivePtr ItemTreeArchiver::store(ArchivePtr parentArchive, ItemPtr item) { itemIdCounter = 0; return storeIter(parentArchive, item.get()); } ArchivePtr ItemTreeArchiver::storeIter(ArchivePtr& parentArchive, Item* item) { string pluginName; string className; if(!ItemManager::getClassIdentifier(item, pluginName, className)){ messageView->putln(format(_("\"%1%\" cannot be stored. Its type is not registered.")) % item->name()); return 0; } ArchivePtr archive = new Archive(); archive->inheritSharedInfoFrom(parentArchive); messageView->putln(format(_("Storing %1% \"%2%\"")) % className % item->name()); messageView->flush(); ArchivePtr dataArchive = new Archive(); dataArchive->inheritSharedInfoFrom(parentArchive); if(!item->store(*dataArchive)){ messageView->putln(format(_("\"%1%\" cannot be stored.")) % item->name()); return 0; } archive->registerItemId(item, itemIdCounter); archive->write("id", itemIdCounter); itemIdCounter++; archive->write("name", item->name(), YAML_DOUBLE_QUOTED); archive->write("plugin", pluginName); archive->write("class", className); if(!dataArchive->empty()){ archive->insert("data", dataArchive); } YamlSequencePtr children = new YamlSequence(); for(Item* childItem = item->childItem(); childItem; childItem = childItem->nextItem()){ if(childItem->isSubItem()){ continue; } ArchivePtr childArchive = storeIter(archive, childItem); if(childArchive){ children->append(childArchive); } } if(!children->empty()){ archive->insert("children", children); } return archive; } bool ItemTreeArchiver::restore(ArchivePtr archive, ItemPtr parentItem) { bool result = false; archive->setCurrentParentItem(0); try { result = restoreItemIter(archive.get(), parentItem); } catch (const YamlNode::Exception& ex){ messageView->put(ex.message()); } archive->setCurrentParentItem(0); return result; } bool ItemTreeArchiver::restoreItemIter(Archive* archive, ItemPtr& parentItem) { ItemPtr item; string pluginName; string className; if(!(archive->read("plugin", pluginName) && archive->read("class", className))){ messageView->putln(_("Archive is broken.")); return false; } const char* actualPluginName = PluginManager::instance()->guessActualPluginName(pluginName); if(actualPluginName){ item = ItemManager::create(actualPluginName, className); } if(!item){ messageView->putln( format(_("Item type %1% of %2% cannot be restored. It's not a registered type.")) % className % pluginName); } else { bool restored = false; string name; if(archive->read("name", name)){ item->setName(name); bool isRootItem = dynamic_pointer_cast(item); if(isRootItem){ item = parentItem; restored = true; } else { messageView->putln(format(_("Restoring %1% \"%2%\"")) % className % name); messageView->flush(); YamlNodePtr dataNode = archive->find("data"); if(!dataNode->isValid()){ restored = true; } else if(dataNode->type() == YAML_MAPPING){ Archive* dataArchive = static_cast(dataNode->toMapping()); dataArchive->inheritSharedInfoFrom(archive); dataArchive->setCurrentParentItem(parentItem.get()); restored = item->restore(*dataArchive); } if(restored){ parentItem->addChildItem(item); } } if(!restored){ messageView->putln(format(_("%1% \"%2%\" cannot be restored.")) % className % name); item = 0; } else { int id; if(archive->read("id", id) && (id >= 0)){ archive->registerItemId(item.get(), id); } YamlSequencePtr children = archive->findSequence("children"); if(children->isValid()){ for(int i=0; i < children->size(); ++i){ Archive* childArchive = dynamic_cast(children->get(i).toMapping()); childArchive->inheritSharedInfoFrom(archive); restoreItemIter(childArchive, item); } } } } } return (item); } choreonoid-1.1.0+dfsg/src/Base/ItemTreeArchiver.h000066400000000000000000000013451207742442300216050ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_ITEM_TREE_ARCHIVER_H_INCLUDED #define CNOID_GUIBASE_ITEM_TREE_ARCHIVER_H_INCLUDED #include "Item.h" #include "Archive.h" #include "exportdecl.h" namespace cnoid { class MessageView; class CNOID_EXPORT ItemTreeArchiver { public: ItemTreeArchiver(); ~ItemTreeArchiver(); ArchivePtr store(ArchivePtr parentArchive, ItemPtr topItem); bool restore(ArchivePtr archive, ItemPtr parentItem); private: ArchivePtr storeIter(ArchivePtr& parentArchive, Item* item); bool restoreItemIter(Archive* archive, ItemPtr& parentItem); MessageView* messageView; int itemIdCounter; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ItemTreeView.cpp000066400000000000000000000610051207742442300213060ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ItemTreeView.h" #include "Item.h" #include "RootItem.h" #include "ExtensionManager.h" #include "MenuManager.h" #include "Archive.h" #include "TreeWidget.h" #include "ConnectionSet.h" #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { // Item of the item tree view class ItvItem : public QTreeWidgetItem { public: ItvItem(Item* item, ItemTreeViewImpl* itemTreeViewImpl); virtual ~ItvItem(); virtual QVariant data(int column, int role) const; virtual void setData(int column, int role, const QVariant& value); ItemPtr item; boost::signal sigCheckToggled; ItemTreeViewImpl* itemTreeViewImpl; bool isExpandedBeforeRemoving; }; // Preserved as custom data in an item class ItvItemRef : public Referenced { public: ItvItemRef(ItvItem* itvItem) : itvItem(itvItem) { } ItvItem* itvItem; }; } namespace cnoid { class ItemTreeViewImpl : public TreeWidget, public boost::signals::trackable { public: ItemTreeViewImpl(ItemTreeView* self, RootItem* rootItem, bool showRoot); ~ItemTreeViewImpl(); ItemTreeView* self; RootItemPtr rootItem; int isProceccingSlotForRootItemSignals; ConnectionSet connectionsFromRootItem; boost::signal sigCheckToggled; boost::signal sigCheckToggledForInvalidItem; boost::signal&)> sigSelectionChanged; boost::signal&)> sigSelectionOrTreeChanged; ItemList selectedItemList; ItemList copiedItemList; ItemList checkedItemList; bool needToUpdateCheckedItemList; Menu* popupMenu; MenuManager menuManager; bool isDropping; virtual void mousePressEvent(QMouseEvent* event); ItvItem* getItvItem(Item* item); ItvItem* getOrCreateItvItem(Item* item); void onItemAdded(Item* item); void addItem(QTreeWidgetItem* parentTwItem, Item* item); void onItemRemoved(Item* item, bool isMoving); void onTreeChanged(); virtual void dropEvent(QDropEvent* event); void onRowsAboutToBeRemoved(const QModelIndex& parent, int start, int end); void onRowsInserted(const QModelIndex& parent, int start, int end); virtual bool dropMimeData(QTreeWidgetItem* parent, int index, const QMimeData* data, Qt::DropAction action); void onSelectionChanged(); bool isItemSelected(Item* item); bool selectItem(Item* item, bool select); bool isItemChecked(Item* item); bool checkItem(Item* item, bool checked); void extractSelectedItemsOfSubTreeTraverse(Item* item, ItemListBase* io_items); ItemList& checkedItems(); void extractCheckedItems(QTreeWidgetItem* twItem); void forEachTopItems(const ItemList& orgItemList, function callback); void forEachTopItemsSub(ItemPtr item, ItemList& items); void cutSelectedItems(); void copySelectedItems(); ItemPtr duplicateItemsInGivenList(ItemPtr item, ItemPtr duplicated, ItemList& items); void copySelectedItemsWithChildren(); void addCopiedItemToCopiedItemList(Item* item); void pasteItems(); void moveCutItemsToCopiedItemList(Item* item); bool storeState(Archive& archive); void storeItemIds(Archive& archive, const char* key, const ItemList& items); bool restoreState(const Archive& archive); void restoreItemStates(const Archive& archive, const char* key, function stateChangeFunc); void storeExpandedItems(Archive& archive); void storeExpandedItemsSub(QTreeWidgetItem* parentTwItem, Archive& archive, YamlSequencePtr& expanded); void restoreExpandedItems(const Archive& archive); }; } ItvItem::ItvItem(Item* item, ItemTreeViewImpl* itemTreeViewImpl) : item(item), itemTreeViewImpl(itemTreeViewImpl) { setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable | Qt::ItemIsUserCheckable); if(!item->isSubItem()){ setFlags(flags() | Qt::ItemIsEditable | Qt::ItemIsDragEnabled | Qt::ItemIsDropEnabled); } setCheckState(1, Qt::Unchecked); ItvItemRef* ref = new ItvItemRef(this); item->setCustomData(0, ref); isExpandedBeforeRemoving = false; } ItvItem::~ItvItem() { item->clearCustomData(0); } QVariant ItvItem::data(int column, int role) const { if((role == Qt::DisplayRole || role == Qt::EditRole) && column == 0){ return item->name().c_str(); } else { return QTreeWidgetItem::data(column, role); } } void ItvItem::setData(int column, int role, const QVariant& value) { bool emitCheckToggled = false; if(column == 0){ if(role == Qt::DisplayRole || role == Qt::EditRole){ if(value.type() == QVariant::String){ if(!value.toString().isEmpty()){ item->setName(value.toString().toStdString()); } } } } else if(column == 1 && role == Qt::CheckStateRole){ emitCheckToggled = true; } QTreeWidgetItem::setData(column, role, value); if(emitCheckToggled){ itemTreeViewImpl->needToUpdateCheckedItemList = true; bool checked = ((Qt::CheckState)value.toInt() == Qt::Checked); itemTreeViewImpl->sigCheckToggled(item.get(), checked); sigCheckToggled(checked); } } void ItemTreeView::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ext->addView(mainInstance()); initialized = true; } } ItemTreeView* ItemTreeView::mainInstance() { static ItemTreeView* itemTreeView = new ItemTreeView(RootItem::mainInstance()); return itemTreeView; } ItemTreeView::ItemTreeView(RootItem* rootItem, bool showRoot) { setName(N_("Items")); setDefaultLayoutArea(View::LEFT); impl = new ItemTreeViewImpl(this, rootItem, showRoot); QVBoxLayout* layout = new QVBoxLayout(); layout->addWidget(impl); setLayout(layout); } ItemTreeViewImpl::ItemTreeViewImpl(ItemTreeView* self, RootItem* rootItem, bool showRoot) : self(self), rootItem(rootItem) { isProceccingSlotForRootItemSignals = 0; needToUpdateCheckedItemList = false; isDropping = false; setColumnCount(2); header()->setStretchLastSection(false); header()->setResizeMode(0, QHeaderView::Stretch); header()->setResizeMode(1, QHeaderView::ResizeToContents); header()->swapSections(0, 1); setWordWrap(true); setFrameShape(QFrame::NoFrame); setHeaderHidden(true); setIndentation(12); setSelectionMode(QAbstractItemView::ExtendedSelection); setDragDropMode(QAbstractItemView::InternalMove); sigItemSelectionChanged().connect(bind(&ItemTreeViewImpl::onSelectionChanged, this)); connectionsFromRootItem.add( rootItem->sigItemAdded().connect(bind(&ItemTreeViewImpl::onItemAdded, this, _1))); connectionsFromRootItem.add( rootItem->sigItemRemoved().connect(bind(&ItemTreeViewImpl::onItemRemoved, this, _1, _2))); connectionsFromRootItem.add( rootItem->sigTreeChanged().connect(bind(&ItemTreeViewImpl::onTreeChanged, this))); QObject::connect(model(), SIGNAL(rowsAboutToBeRemoved(const QModelIndex&, int, int)), self, SLOT(onRowsAboutToBeRemoved(const QModelIndex&, int, int))); QObject::connect(model(), SIGNAL(rowsInserted(const QModelIndex&, int, int)), self, SLOT(onRowsInserted(const QModelIndex&, int, int))); popupMenu = new Menu(this); menuManager.setTopMenu(popupMenu); menuManager.addItem(_("Cut")) ->sigTriggered().connect(bind(&ItemTreeViewImpl::cutSelectedItems, this)); menuManager.addItem(_("Copy")) ->sigTriggered().connect(bind(&ItemTreeViewImpl::copySelectedItems, this)); menuManager.addItem(_("Copy with Children")) ->sigTriggered().connect(bind(&ItemTreeViewImpl::copySelectedItemsWithChildren, this)); menuManager.addItem(_("Paste")) ->sigTriggered().connect(bind(&ItemTreeViewImpl::pasteItems, this)); } ItemTreeView::~ItemTreeView() { } ItemTreeViewImpl::~ItemTreeViewImpl() { // On Windows + VC++, boost::signal::trackable, the super class of // ItemTreeViewImpl, does not seem to work correctly, and the connection // is not disconnected and the program aborted by segmentation fault. // So the following explicit disconnection code is added. //connectionClassUnregistered.disconnect(); } RootItem* ItemTreeView::rootItem() { return impl->rootItem.get(); } void ItemTreeView::showRoot(bool show) { } void ItemTreeViewImpl::mousePressEvent(QMouseEvent* event) { TreeWidget::mousePressEvent(event); if(event->button() == Qt::RightButton){ popupMenu->popup(event->globalPos()); } } ItvItem* ItemTreeViewImpl::getItvItem(Item* item) { ItvItem* itvItem = 0; ItvItemRef* ref = dynamic_cast(item->customData(0)); if(ref){ itvItem = ref->itvItem; } return itvItem; } ItvItem* ItemTreeViewImpl::getOrCreateItvItem(Item* item) { ItvItem* itvItem = getItvItem(item); if(!itvItem){ itvItem = new ItvItem(item, this); } return itvItem; } void ItemTreeViewImpl::onItemAdded(Item* item) { isProceccingSlotForRootItemSignals++; Item* parentItem = item->parentItem(); if(parentItem){ if(parentItem == rootItem){ addItem(invisibleRootItem(), item); } else { ItvItem* parentItvItem = getItvItem(parentItem); if(parentItvItem){ addItem(parentItvItem, item); } } } isProceccingSlotForRootItemSignals--; } void ItemTreeViewImpl::addItem(QTreeWidgetItem* parentTwItem, Item* item) { ItvItem* itvItem = getOrCreateItvItem(item); parentTwItem->addChild(itvItem); if(!parentTwItem->isExpanded()){ if(!item->isSubItem()){ parentTwItem->setExpanded(true); } } for(Item* childItem = item->childItem(); childItem; childItem = childItem->nextItem()){ addItem(itvItem, childItem); } } void ItemTreeViewImpl::onItemRemoved(Item* item, bool isMoving) { isProceccingSlotForRootItemSignals++; ItvItem* itvItem = getItvItem(item); if(itvItem){ QTreeWidgetItem* parentTwItem = itvItem->parent(); if(parentTwItem){ parentTwItem->removeChild(itvItem); } else { takeTopLevelItem(indexOfTopLevelItem(itvItem)); } delete itvItem; } isProceccingSlotForRootItemSignals--; } void ItemTreeViewImpl::dropEvent(QDropEvent* event) { isDropping = true; TreeWidget::dropEvent(event); isDropping = false; } void ItemTreeView::onRowsAboutToBeRemoved(const QModelIndex& parent, int start, int end) { if(impl->isProceccingSlotForRootItemSignals == 0){ impl->onRowsAboutToBeRemoved(parent, start, end); } } void ItemTreeViewImpl::onRowsAboutToBeRemoved(const QModelIndex& parent, int start, int end) { connectionsFromRootItem.block(); QTreeWidgetItem* parentTwItem = itemFromIndex(parent); if(!parentTwItem){ parentTwItem = invisibleRootItem(); } for(int i=start; i <= end; ++i){ ItvItem* itvItem = dynamic_cast(parentTwItem->child(i)); if(itvItem){ itvItem->isExpandedBeforeRemoving = itvItem->isExpanded(); if(!isDropping){ ItemPtr& item = itvItem->item; if(!item->isSubItem()){ item->detachFromParentItem(); } } } } connectionsFromRootItem.unblock(); } void ItemTreeView::onRowsInserted(const QModelIndex& parent, int start, int end) { if(impl->isProceccingSlotForRootItemSignals == 0){ impl->onRowsInserted(parent, start, end); } } void ItemTreeViewImpl::onRowsInserted(const QModelIndex& parent, int start, int end) { connectionsFromRootItem.block(); QTreeWidgetItem* parentTwItem = itemFromIndex(parent); if(!parentTwItem){ parentTwItem = invisibleRootItem(); } ItvItem* parentItvItem = dynamic_cast(parentTwItem); Item* parentItem = parentItvItem ? parentItvItem->item.get() : rootItem.get(); ItemPtr nextItem = 0; if(end + 1 < parentTwItem->childCount()){ ItvItem* nextItvItem = dynamic_cast(parentTwItem->child(end + 1)); if(nextItvItem){ nextItem = nextItvItem->item; } } for(int i=start; i <= end; ++i){ ItvItem* itvItem = dynamic_cast(parentTwItem->child(i)); if(itvItem){ ItemPtr& item = itvItem->item; if(!item->isSubItem()){ parentItem->insertChildItem(item, nextItem); } if(itvItem->isExpandedBeforeRemoving){ itvItem->setExpanded(true); } } } connectionsFromRootItem.unblock(); } void ItemTreeViewImpl::onTreeChanged() { } bool ItemTreeViewImpl::dropMimeData(QTreeWidgetItem* parent, int index, const QMimeData* data, Qt::DropAction action) { return TreeWidget::dropMimeData(parent, index, data, action); } void ItemTreeViewImpl::onSelectionChanged() { selectedItemList.clear(); QList selected = selectedItems(); for(int i=0; i < selected.size(); ++i){ ItvItem* itvItem = dynamic_cast(selected[i]); if(itvItem){ selectedItemList.append(itvItem->item); } } sigSelectionChanged(selectedItemList); sigSelectionOrTreeChanged(selectedItemList); } bool ItemTreeView::isItemSelected(ItemPtr item) { return impl->isItemSelected(item.get()); } bool ItemTreeViewImpl::isItemSelected(Item* item) { ItvItem* itvItem = getItvItem(item); if(itvItem){ return itvItem->isSelected(); } return false; } bool ItemTreeView::selectItem(ItemPtr item, bool select) { return impl->selectItem(item.get(), select); } bool ItemTreeViewImpl::selectItem(Item* item, bool select) { ItvItem* itvItem = getItvItem(item); if(itvItem){ QModelIndex index = indexFromItem(itvItem); selectionModel()->select(index, (select ? QItemSelectionModel::Select : QItemSelectionModel::Deselect)); return select; } return false; } void ItemTreeView::clearSelection() { impl->selectionModel()->clearSelection(); } bool ItemTreeView::isItemChecked(ItemPtr item) { return impl->isItemChecked(item.get()); } bool ItemTreeViewImpl::isItemChecked(Item* item) { ItvItem* itvItem = getItvItem(item); if(itvItem){ return (itvItem->checkState(1) == Qt::Checked); } return false; } bool ItemTreeView::checkItem(ItemPtr item, bool checked) { return impl->checkItem(item.get(), checked); } bool ItemTreeViewImpl::checkItem(Item* item, bool checked) { ItvItem* itvItem = getItvItem(item); if(itvItem){ itvItem->setCheckState(1, (checked ? Qt::Checked : Qt::Unchecked)); return checked; } return false; } SignalProxy< boost::signal&)> > ItemTreeView::sigSelectionChanged() { return impl->sigSelectionChanged; } SignalProxy< boost::signal&)> > ItemTreeView::sigSelectionOrTreeChanged() { return impl->sigSelectionOrTreeChanged; } SignalProxy< boost::signal > ItemTreeView::sigCheckToggled() { return impl->sigCheckToggled; } SignalProxy< boost::signal > ItemTreeView::sigCheckToggled(Item* targetItem) { return impl->getOrCreateItvItem(targetItem)->sigCheckToggled; } void ItemTreeView::getSelectedItems(ItemListBase& items) { items = impl->selectedItemList; } void ItemTreeView::extractSelectedItemsOfSubTree(ItemPtr topItem, ItemListBase& io_items) { topItem->traverse(bind(&ItemTreeViewImpl::extractSelectedItemsOfSubTreeTraverse, impl, _1, &io_items)); } void ItemTreeViewImpl::extractSelectedItemsOfSubTreeTraverse(Item* item, ItemListBase* io_items) { ItvItem* itvItem = getItvItem(item); if(itvItem && itvItem->isSelected()){ io_items->appendIfTypeMatches(item); } } void ItemTreeView::getCheckedItems(ItemListBase& items) { items = impl->checkedItems(); } ItemList& ItemTreeViewImpl::checkedItems() { if(needToUpdateCheckedItemList){ checkedItemList.clear(); extractCheckedItems(invisibleRootItem()); needToUpdateCheckedItemList = false; } return checkedItemList; } void ItemTreeViewImpl::extractCheckedItems(QTreeWidgetItem* twItem) { ItvItem* itvItem = dynamic_cast(twItem); if(itvItem){ if(itvItem->checkState(1) == Qt::Checked){ checkedItemList.append(itvItem->item); } } int n = twItem->childCount(); for(int i=0; i < n; ++i){ extractCheckedItems(twItem->child(i)); } } void ItemTreeViewImpl::forEachTopItems(const ItemList& orgItemList, function callback) { ItemList items(orgItemList); while(!items.empty()){ ItemPtr item = items.front(); items.pop_front(); forEachTopItemsSub(item, items); callback(item.get()); } } void ItemTreeViewImpl::forEachTopItemsSub(ItemPtr item, ItemList& items) { for(ItemPtr childItem = item->childItem(); (childItem && !items.empty()); childItem = childItem->nextItem()){ if(childItem == items.front()){ items.pop_front(); } forEachTopItemsSub(childItem, items); } } void ItemTreeViewImpl::cutSelectedItems() { copiedItemList.clear(); forEachTopItems(selectedItemList, bind(&ItemTreeViewImpl::moveCutItemsToCopiedItemList, this, _1)); } void ItemTreeViewImpl::copySelectedItems() { copiedItemList.clear(); ItemList items(selectedItemList); while(!items.empty()){ ItemPtr item = items.front(); items.pop_front(); ItemPtr duplicated = duplicateItemsInGivenList(item, 0, items); if(duplicated){ copiedItemList.push_back(duplicated); } } } ItemPtr ItemTreeViewImpl::duplicateItemsInGivenList(ItemPtr item, ItemPtr duplicated, ItemList& items) { if(!duplicated){ duplicated = item->duplicate(); } for(ItemPtr childItem = item->childItem(); (childItem && !items.empty()); childItem = childItem->nextItem()){ if(childItem == items.front()){ items.pop_front(); ItemPtr duplicatedChild; if(childItem->isSubItem()){ duplicatedChild = duplicated->findItem(childItem->name()); if(duplicatedChild){ duplicateItemsInGivenList(childItem, duplicatedChild, items); } } else { duplicatedChild = duplicateItemsInGivenList(childItem, 0, items); if(duplicatedChild){ duplicated->addChildItem(duplicatedChild); } } } } return duplicated; } void ItemTreeViewImpl::copySelectedItemsWithChildren() { copiedItemList.clear(); forEachTopItems(selectedItemList, bind(&ItemTreeViewImpl::addCopiedItemToCopiedItemList, this, _1)); } void ItemTreeViewImpl::addCopiedItemToCopiedItemList(Item* item) { ItemPtr duplicated = item->duplicateAll(); if(duplicated){ copiedItemList.push_back(duplicated); } } void ItemTreeViewImpl::pasteItems() { if(!copiedItemList.empty()){ ItemPtr parentItem; if(selectedItemList.empty()){ parentItem = rootItem; } else if(selectedItemList.size() == 1){ parentItem = selectedItemList.front(); } if(parentItem){ for(size_t i=0; i < copiedItemList.size(); ++i){ parentItem->addChildItem(copiedItemList[i]->duplicateAll()); } } } } void ItemTreeViewImpl::moveCutItemsToCopiedItemList(Item* item) { if(!item->isSubItem()){ copiedItemList.push_back(item); item->detachFromParentItem(); } } bool ItemTreeView::storeState(Archive& archive) { return impl->storeState(archive); } bool ItemTreeViewImpl::storeState(Archive& archive) { storeItemIds(archive, "selected", selectedItemList); storeItemIds(archive, "checked", checkedItems()); storeExpandedItems(archive); return true; } void ItemTreeViewImpl::storeItemIds(Archive& archive, const char* key, const ItemList& items) { YamlSequencePtr idseq = new YamlSequence(); idseq->setFlowStyle(true); for(size_t i=0; i < items.size(); ++i){ int id = archive.getItemId(items[i]); if(id >= 0){ idseq->append(id); } } if(!idseq->empty()){ archive.insert(key, idseq); } } bool ItemTreeView::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool ItemTreeViewImpl::restoreState(const Archive& archive) { restoreItemStates(archive, "checked", bind(&ItemTreeView::checkItem, self, _1, true)); restoreItemStates(archive, "selected", bind(&ItemTreeView::selectItem, self, _1, true)); restoreExpandedItems(archive); return true; } void ItemTreeViewImpl::restoreItemStates (const Archive& archive, const char* key, function stateChangeFunc) { const YamlSequence& idseq = *archive.findSequence(key); if(idseq.isValid()){ for(int i=0; i < idseq.size(); ++i){ int id = idseq[i].toInt(); if(id >= 0){ ItemPtr item = archive.findItem(id); if(item){ stateChangeFunc(item); } } } } } void ItemTreeViewImpl::storeExpandedItems(Archive& archive) { YamlSequencePtr expanded = new YamlSequence(); expanded->setFlowStyle(true); storeExpandedItemsSub(invisibleRootItem(), archive, expanded); if(!expanded->empty()){ archive.insert("expanded", expanded); } } void ItemTreeViewImpl::storeExpandedItemsSub(QTreeWidgetItem* parentTwItem, Archive& archive, YamlSequencePtr& expanded) { int n = parentTwItem->childCount(); for(int i=0; i < n; ++i){ ItvItem* itvItem = dynamic_cast(parentTwItem->child(i)); if(itvItem){ if(itvItem->isExpanded()){ Item* item = itvItem->item.get(); if(!item->isSubItem()){ int id = archive.getItemId(item); if(id >= 0){ expanded->append(id); } } else { int j = 0; Item* p = item; while(p->isSubItem()){ ++j; p = p->parentItem(); } int id = archive.getItemId(p); if(id >= 0){ YamlSequencePtr path = new YamlSequence(j+1); path->setFlowStyle(true); while(item->isSubItem()){ path->write(j--, item->name(), YAML_DOUBLE_QUOTED); item = item->parentItem(); } path->write(0, id); expanded->append(path); } } } if(itvItem->childCount() > 0){ storeExpandedItemsSub(itvItem, archive, expanded); } } } } void ItemTreeViewImpl::restoreExpandedItems(const Archive& archive) { const YamlSequence& expanded = *archive.findSequence("expanded"); if(expanded.isValid()){ collapseAll(); for(int i=0; i < expanded.size(); ++i){ Item* item = 0; if(!expanded[i].isSequence()){ item = archive.findItem(expanded[i].toInt()); } else { const YamlSequence& path = *expanded[i].toSequence(); int n = path.size(); if(n >= 2){ item = archive.findItem(path[0].toInt()); for(int j=1; item && j < n; ++j){ item = item->findItem(path[j].toString()); } } } if(item){ ItvItem* itvItem = getItvItem(item); if(itvItem){ itvItem->setExpanded(true); } } } } } choreonoid-1.1.0+dfsg/src/Base/ItemTreeView.h000066400000000000000000000076641207742442300207660ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_ITEM_TREE_VIEW_H_INCLUDED #define CNOID_GUIBASE_ITEM_TREE_VIEW_H_INCLUDED #include "ItemList.h" #include "SignalProxy.h" #include #include #include "exportdecl.h" namespace cnoid { class RootItem; class ItemTreeViewImpl; /** @if jp アイテムツリーを表示ã™ã‚‹ã‚¦ã‚£ãƒ³ãƒ‰ã‚¦ @endif */ class CNOID_EXPORT ItemTreeView : public View { Q_OBJECT public: static void initialize(ExtensionManager* ext); static ItemTreeView* mainInstance(); ItemTreeView(RootItem* rootItem, bool showRoot = false); ~ItemTreeView(); RootItem* rootItem(); void showRoot(bool show); /** @if jp é¸æŠžçŠ¶æ…‹ã«ãªã£ã¦ã„るアイテムã®ã†ã¡ã€æŒ‡å®šã—ãŸåž‹ã«é©åˆã™ã‚‹ã‚‚ã®ã‚’å–å¾—ã™ã‚‹ã€‚ @endif */ template inline ItemList selectedItems() { ItemList items; getSelectedItems(items); return items; } template inline ItemType* selectedItem(bool fromMultiItems = false) { return selectedItems().toSingle(fromMultiItems).get(); } /** @if jp topItem 以下ã®ã‚µãƒ–ツリーã«ãŠã‘ã‚‹é¸æŠžçŠ¶æ…‹ã‚¢ã‚¤ãƒ†ãƒ ã®ãƒªã‚¹ãƒˆã‚’得る。 topItem ã¯é¸æŠžã•ã‚Œã¦ã„ã¦ã‚‚リストã«ã¯å«ã¾ã‚Œãªã„。 @endif */ template inline ItemList selectedSubItems(ItemPtr topItem) { ItemList items; extractSelectedItemsOfSubTree(topItem, items); return items; } template inline ItemType* selectedSubItem(ItemPtr topItem, bool fromMultiItems = false) { return selectedSubItems(topItem).toSingle(fromMultiItems).get(); } bool isItemSelected(ItemPtr item); bool selectItem(ItemPtr item, bool select = true); void clearSelection(); /** @if jp ãƒã‚§ãƒƒã‚¯çŠ¶æ…‹ã«ãªã£ã¦ã„るアイテムã®ã†ã¡ã€æŒ‡å®šã—ãŸåž‹ã«é©åˆã™ã‚‹ã‚‚ã®ã‚’å–å¾—ã™ã‚‹ã€‚ @endif */ template inline ItemList checkedItems() { ItemList items; getCheckedItems(items); return items; } bool isItemChecked(ItemPtr item); bool checkItem(ItemPtr item, bool check); /** @if jp アイテムã®é¸æŠžçŠ¶æ…‹ãŒå¤‰åŒ–ã—ãŸã¨ãã«ç™ºè¡Œã•ã‚Œã‚‹ã‚·ã‚°ãƒŠãƒ«ã€‚ @endif */ SignalProxy< boost::signal&)> > sigSelectionChanged(); /** @if jp アイテムã®é¸æŠžçŠ¶æ…‹ãŒå¤‰åŒ–ã—ãŸã‹ã€ãƒ„リーã®æ§‹é€ ãŒå¤‰åŒ–ã—ãŸã¨ãã«ç™ºè¡Œã•ã‚Œã‚‹ã‚·ã‚°ãƒŠãƒ«ã€‚ アイテム間ã®è¦ªå­é–¢ä¿‚ã‚‚ã¿ã‚‹ã‚ˆã†ãªãƒãƒ³ãƒ‰ãƒ©ã¯ã“ã®ã‚·ã‚°ãƒŠãƒ«ã¨æŽ¥ç¶šã™ã‚‹ã¨ã‚ˆã„。 @endif */ SignalProxy< boost::signal&)> > sigSelectionOrTreeChanged(); SignalProxy< boost::signal > sigCheckToggled(); SignalProxy< boost::signal > sigCheckToggled(Item* targetItem); protected: virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); private: ItemTreeViewImpl* impl; void getSelectedItems(ItemListBase& items); void getCheckedItems(ItemListBase& items); void extractSelectedItemsOfSubTree(ItemPtr topItem, ItemListBase& items); private Q_SLOTS: void onRowsAboutToBeRemoved(const QModelIndex& parent, int start, int end); void onRowsInserted(const QModelIndex& parent, int start, int end); }; } #endif choreonoid-1.1.0+dfsg/src/Base/LazyCaller.cpp000066400000000000000000000055461207742442300210070ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "LazyCaller.h" #include #include #include #include #include #include using namespace cnoid; namespace { class CallEvent : public QEvent { public: CallEvent(const boost::function& function) : QEvent(QEvent::User), function(function) { } boost::function function; }; class SynchronousCallEvent : public QEvent { public: SynchronousCallEvent(const boost::function& function, QSemaphore& semaphore) : QEvent(QEvent::User), function(function), semaphore(semaphore) { } boost::function function; QSemaphore& semaphore; }; class CallEventHandler : public QObject { public: CallEventHandler() { mainThreadId = QThread::currentThreadId(); } virtual bool event(QEvent* e); Qt::HANDLE mainThreadId; }; CallEventHandler callEventHandler; } void cnoid::callLater(const boost::function& function, int priority) { QCoreApplication::postEvent(&callEventHandler, new CallEvent(function), priority); } void cnoid::callSynchronously(const boost::function& function, int priority) { if(QThread::currentThreadId() == callEventHandler.mainThreadId){ callLater(function, priority); } else { QSemaphore semaphore; QCoreApplication::postEvent(&callEventHandler, new SynchronousCallEvent(function, semaphore), priority); semaphore.acquire(); // wait for finish } } bool CallEventHandler::event(QEvent* e) { CallEvent* callEvent = dynamic_cast(e); if(callEvent){ callEvent->function(); return true; } else { SynchronousCallEvent* scEvent = dynamic_cast(e); if(scEvent){ scEvent->function(); scEvent->semaphore.release(); // wake up return true; } } return false; } LazyCaller::LazyCaller() { isCallEventPending = false; priority = IDLE_PRIORITY_HIGH; } LazyCaller::LazyCaller(const boost::function& function, int priority) : function(function), priority(priority) { isCallEventPending = false; } void LazyCaller::set(const boost::function& function, int priority) { this->function = function; this->priority = priority; } void LazyCaller::setPriority(int priority) { this->priority = priority; } void LazyCaller::request() { if(!isCallEventPending){ isCallEventPending = true; callLater(boost::bind(&LazyCaller::onRequest, this), priority); } } void LazyCaller::onRequest() { isCallEventPending = false; function(); } choreonoid-1.1.0+dfsg/src/Base/LazyCaller.h000066400000000000000000000021731207742442300204450ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_LAZY_CALLER_H_INCLUDED #define CNOID_GUIBASE_LAZY_CALLER_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { enum { IDLE_PRIORITY_HIGH = Qt::HighEventPriority, IDLE_PRIORITY_NORMAL = Qt::NormalEventPriority, IDLE_PRIORITY_LOW = Qt::LowEventPriority }; class CNOID_EXPORT LazyCaller { boost::function function; bool isCallEventPending; int priority; public: LazyCaller(); LazyCaller(const boost::function& function, int priority = IDLE_PRIORITY_HIGH); void set(const boost::function& function, int priority = IDLE_PRIORITY_HIGH); void setPriority(int priority); void request(); private: void onRequest(); }; CNOID_EXPORT void callLater(const boost::function& function, int priority = IDLE_PRIORITY_NORMAL); CNOID_EXPORT void callSynchronously(const boost::function& function, int priority = IDLE_PRIORITY_NORMAL); } #endif choreonoid-1.1.0+dfsg/src/Base/LazySignal.cpp000066400000000000000000000013231207742442300210070ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "LazySignal.h" #include using namespace cnoid; void LazySignalBase::request() { if(!isIdleEventPending){ isIdleEventPending = true; callLater(boost::bind(&LazySignalBase::onIdle, this), priority); } } bool LazySignalBase::onIdle() { isIdleEventPending = false; for(size_t i=0; i < connectionsToBlock.size(); ++i){ connectionsToBlock[i].block(); } if(emitFunction){ emitFunction(); } else { defaultEmitFunction(); } for(size_t i=0; i < connectionsToBlock.size(); ++i){ connectionsToBlock[i].unblock(); } connectionsToBlock.clear(); return false; } choreonoid-1.1.0+dfsg/src/Base/LazySignal.h000066400000000000000000000031741207742442300204620ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_LAZY_SIGNAL_H_INCLUDED #define CNOID_GUIBASE_LAZY_SIGNAL_H_INCLUDED #include "LazyCaller.h" #include #include #include #include "exportdecl.h" namespace cnoid { // Base class for hiding the boost.bind header into the cpp file. class CNOID_EXPORT LazySignalBase { public: void request(); protected: boost::function emitFunction; int priority; bool isIdleEventPending; std::vector connectionsToBlock; virtual void defaultEmitFunction() = 0; private: bool onIdle(); }; template class LazySignal : public LazySignalBase { public: LazySignal(int priority = IDLE_PRIORITY_HIGH) { this->priority = priority; isIdleEventPending = false; } LazySignal(boost::function emitFunction, int priority = IDLE_PRIORITY_HIGH) { this->emitFunction = emitFunction; this->priority = priority; isIdleEventPending = false; } inline SignalType& signal() { return signal_; } inline void requestBlocking(boost::signals::connection connection){ connectionsToBlock.push_back(connection); } inline bool isBeingRequested() { return isIdleEventPending; } protected: virtual void defaultEmitFunction() { signal_(); } private: SignalType signal_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/Licenses.cpp000066400000000000000000000012431207742442300205000ustar00rootroot00000000000000 #include "Licenses.h" #include "gettext.h" const char* cnoid::LGPLtext() { return _("This program is free software; you can redistribute it and/or modify it under the terms of " "the GNU Lesser General Public License as published by the Free Software Foundation; either " "version 2.1 of the License, or (at your option) any later version.\n" "\n" "This package is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; " "without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE." "See the GNU Lesser General Public License for more details.\n"); } choreonoid-1.1.0+dfsg/src/Base/Licenses.h000066400000000000000000000002571207742442300201510ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BASE_LICENSES_H_INCLUDED #define CNOID_BASE_LICENSES_H_INCLUDED namespace cnoid { const char* LGPLtext(); } #endif choreonoid-1.1.0+dfsg/src/Base/LineEdit.cpp000066400000000000000000000026171207742442300204360ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "LineEdit.h" using namespace cnoid; LineEdit::LineEdit(QWidget* parent) : QLineEdit(parent) { initialize(); } LineEdit::LineEdit(const QString& contents, QWidget* parent) : QLineEdit(contents, parent) { initialize(); } void LineEdit::initialize() { connect(this, SIGNAL(cursorPositionChanged(int, int)), this, SLOT(onCursorPositionChanged(int, int))); connect(this, SIGNAL(editingFinished()), this, SLOT(onEditingFinished())); connect(this, SIGNAL(returnPressed()), this, SLOT(onReturnPressed())); connect(this, SIGNAL(selectionChanged()), this, SLOT(onSelectionChanged())); connect(this, SIGNAL(textChanged(const QString&)), this, SLOT(onTextChanged(const QString&))); connect(this, SIGNAL(textEdited(const QString&)), this, SLOT(onTextEdited(const QString&))); } void LineEdit::onCursorPositionChanged(int oldpos, int newpos) { sigCursorPositionChanged_(oldpos, newpos); } void LineEdit::onEditingFinished() { sigEditingFinished_(); } void LineEdit::onReturnPressed() { sigReturnPressed_(); } void LineEdit::onSelectionChanged() { sigSelectionChanged_(); } void LineEdit::onTextChanged(const QString& text) { sigTextChanged_(text); } void LineEdit::onTextEdited(const QString& text) { sigTextEdited_(text); } choreonoid-1.1.0+dfsg/src/Base/LineEdit.h000066400000000000000000000036131207742442300201000ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_LINE_EDIT_H_INCLUDED #define CNOID_GUIBASE_LINE_EDIT_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT LineEdit : public QLineEdit { Q_OBJECT public: LineEdit(QWidget* parent = 0); LineEdit(const QString& contents, QWidget* parent = 0); inline SignalProxy< boost::signal > sigCursorPositoinChanged() { return sigCursorPositionChanged_; } inline SignalProxy< boost::signal > sigEditingFinished() { return sigEditingFinished_; } inline SignalProxy< boost::signal > sigReturnPressed() { return sigReturnPressed_; } inline SignalProxy< boost::signal > sigSelectionChanged() { return sigSelectionChanged_; } inline SignalProxy< boost::signal > sigTextChanged() { return sigTextChanged_; } inline SignalProxy< boost::signal > sigTextEdited() { return sigTextEdited_; } private Q_SLOTS: void onCursorPositionChanged(int oldpos, int newpos); void onEditingFinished(); void onReturnPressed(); void onSelectionChanged(); void onTextChanged(const QString& text); void onTextEdited(const QString& text); private: boost::signal sigCursorPositionChanged_; boost::signal sigEditingFinished_; boost::signal sigReturnPressed_; boost::signal sigSelectionChanged_; boost::signal sigTextChanged_; boost::signal sigTextEdited_; void initialize(); }; } #endif choreonoid-1.1.0+dfsg/src/Base/MainWindow.cpp000066400000000000000000001075601207742442300210200ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "MainWindow.h" #include "App.h" #include "View.h" #include "ToolBar.h" #include "ToolBarArea.h" #include "InfoBar.h" #include "MenuManager.h" #include "AppConfig.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; MainWindow* mainWindow = 0; enum DropArea { OVER = -1, LEFT = 0, TOP, RIGHT, BOTTOM, NUM_DROP_AREAS }; const int SPLIT_DISTANCE_THRESHOLD = 35; class TabWidget : public QTabWidget { public: TabWidget(MainWindowImpl* mwi, QWidget* parent = 0); int addView(View* view) { return addTab(view, view->windowTitle()); } QTabBar* tabBar() const { return QTabWidget::tabBar(); } virtual bool eventFilter(QObject* object, QEvent* event); // For the whole tab widget dragging //virtual void mousePressEvent(QMouseEvent *event); //virtual void mouseMoveEvent(QMouseEvent *event); //virtual void mouseReleaseEvent(QMouseEvent *event); MainWindowImpl* mwi; }; } namespace cnoid { class MainWindowImpl //: public boost::signals::trackable { public: MainWindow* self; MainWindowImpl(MainWindow* self, const char* appName, ExtensionManager* ext); ~MainWindowImpl(); void setupMenus(ExtensionManager* ext); void createDefaultPanes(); bool addView(const std::string& pluginName, View* view); bool removeView(View* view); void showFirst(); void onFullScreenToggled(bool on); void resizeEvent(QResizeEvent* event); void keyPressEvent(QKeyEvent* event); bool viewTabMousePressEvent(TabWidget* pane, QMouseEvent* event); bool viewTabMouseMoveEvent(TabWidget* pane, QMouseEvent* event); bool viewTabMouseReleaseEvent(TabWidget* pane, QMouseEvent *event); void startViewDrag(View* view); void dragView(QMouseEvent* event); void dragViewInsidePane(const QPoint& posInDestPane); void dropViewInsidePane(); void dragViewOutsidePane(); void dropViewOutsidePane(); void removePaneIfEmpty(TabWidget* pane); void clearAllPanes(); void clearAllPanesSub(QSplitter* splitter); void clearEmptyPanes(); void onSaveLayout(); void onSaveLayoutAs(); void storeLayout(YamlMappingPtr layout); YamlMapping* storeSplitterState(QSplitter* splitter); YamlMapping* storePaneState(TabWidget* pane); void onLoadLayout(); void onLoadLayoutAs(); void restoreLayout(const YamlMappingPtr layout); QWidget* restoreSplitterState(const YamlMapping& state, TabWidget*& out_firstPane); TabWidget* restorePaneState(const YamlMapping& state); void restoreDefaultLayout(); std::vector views; typedef multimap NameToViewMap; NameToViewMap nameToViewMap; typedef set ViewSet; ViewSet storedViews; std::vector toolBars; TabWidget* areaToPane[View::NUM_AREAS]; struct AreaDetectionInfo { AreaDetectionInfo() { for(int i=0; i < View::NUM_AREAS; ++i){ scores[i] = 0; } } TabWidget* pane; int scores[View::NUM_AREAS]; }; typedef bitset EdgeContactState; ToolBarArea* toolBarArea; QWidget* centralWidget; QVBoxLayout* centralVBox; QSplitter* topSplitter; YamlMappingPtr config; YamlMappingPtr initialLayout; bool isBeforeShowing; bool isBeforeDoingInitialLayout; bool isMaximized; bool isMaximizedJustBeforeFullScreen; bool isFullScreen; QSize normalStateSize; QString currentLayoutFolder; Action* fullScreenCheck; QPoint tabDragStartPosition; bool isViewDragging; TabWidget* dragSrcPane; TabWidget* dragDestPane; bool isViewDraggingOutsidePane; int dropEdge; QRubberBand* rubberBand; }; } TabWidget::TabWidget(MainWindowImpl* mwi, QWidget* parent) : QTabWidget(parent), mwi(mwi) { setMovable(true); setUsesScrollButtons(true); tabBar()->installEventFilter(this); } bool TabWidget::eventFilter(QObject* object, QEvent* event) { if(object == tabBar()){ switch(event->type()){ case QEvent::MouseButtonPress: return mwi->viewTabMousePressEvent(this, static_cast(event)); case QEvent::MouseButtonDblClick: break; case QEvent::MouseButtonRelease: return mwi->viewTabMouseReleaseEvent(this, static_cast(event)); case QEvent::MouseMove: return mwi->viewTabMouseMoveEvent(this, static_cast(event)); default: break; } } return false; } void MainWindow::initialize(const char* appName, ExtensionManager* ext) { if(!mainWindow){ new MainWindow(appName, ext); } } MainWindow* MainWindow::instance() { return mainWindow; } MainWindow::MainWindow(const char* appName, ExtensionManager* ext) { mainWindow = this; setWindowTitle(appName); setFocusPolicy(Qt::WheelFocus); impl = new MainWindowImpl(this, appName, ext); } MainWindowImpl::MainWindowImpl(MainWindow* self, const char* appName, ExtensionManager* ext) : self(self) { isBeforeDoingInitialLayout = true; isMaximized = false; isViewDragging = false; centralWidget = new QWidget(self); centralVBox = new QVBoxLayout(centralWidget); centralVBox->setSpacing(0); centralVBox->setContentsMargins(0, 0, 0, 0); toolBarArea = new ToolBarArea(centralWidget); centralVBox->addWidget(toolBarArea); //centralVBox->addSpacing(2); topSplitter = new QSplitter(centralWidget); centralVBox->addWidget(topSplitter, 1); self->setCentralWidget(centralWidget); rubberBand = new QRubberBand(QRubberBand::Rectangle, centralWidget); rubberBand->hide(); config = AppConfig::archive()->openMapping("MainWindow"); createDefaultPanes(); self->setStatusBar(InfoBar::instance()); setupMenus(ext); initialLayout = config; toolBarArea->setInitialLayout(config); isBeforeShowing = true; if(TRACE_FUNCTIONS){ cout << "size = (" << self->width() << ", " << self->height() << ")" << endl; } normalStateSize.setWidth(config->get("width", self->width())); normalStateSize.setHeight(config->get("height", self->width())); } MainWindow::~MainWindow() { if(impl){ delete impl; impl = 0; } } MainWindowImpl::~MainWindowImpl() { config->write("fullScreen", self->isFullScreen()); config->write("maximized", isMaximized); config->write("width", normalStateSize.width()); config->write("height", normalStateSize.height()); } void MainWindowImpl::setupMenus(ExtensionManager* ext) { MenuManager& mm = ext->menuManager(); mm.setPath("/" N_("File")).setBackwardMode().addItem(_("Exit")) ->sigTriggered().connect(bind(&MainWindow::close, self)); mm.setPath("/" N_("Edit")); mm.setPath("/" N_("View")); fullScreenCheck = mm.addCheckItem(_("Full Screen")); fullScreenCheck->setChecked(config->get("fullScreen", false)); fullScreenCheck->sigToggled().connect(bind(&MainWindowImpl::onFullScreenToggled, this, _1)); mm.setPath("/View").setPath(N_("Layout")); mm.addItem(_("Save Default Layout")) ->sigTriggered().connect(bind(&MainWindowImpl::onSaveLayout, this)); mm.addItem(_("Load Default Layout")) ->sigTriggered().connect(bind(&MainWindowImpl::onLoadLayout, this)); mm.addItem(_("Save Layout As")) ->sigTriggered().connect(bind(&MainWindowImpl::onSaveLayoutAs, this)); mm.addItem(_("Load Layout As")) ->sigTriggered().connect(bind(&MainWindowImpl::onLoadLayoutAs, this)); mm.setPath("/" N_("Tools")); mm.setPath("/" N_("Filters")); mm.setPath("/" N_("Options")); mm.setPath("/").setBackwardMode().setPath(N_("Help")); } void MainWindowImpl::createDefaultPanes() { topSplitter->setOrientation(Qt::Horizontal); QSplitter* vSplitter0 = new QSplitter(Qt::Vertical, topSplitter); topSplitter->addWidget(vSplitter0); areaToPane[View::LEFT_TOP] = new TabWidget(this, vSplitter0); vSplitter0->addWidget(areaToPane[View::LEFT_TOP]); areaToPane[View::LEFT_BOTTOM] = new TabWidget(this, vSplitter0); vSplitter0->addWidget(areaToPane[View::LEFT_BOTTOM]); QSplitter* vSplitter1 = new QSplitter(Qt::Vertical, topSplitter); topSplitter->addWidget(vSplitter1); QSplitter* hSplitter1 = new QSplitter(Qt::Horizontal, vSplitter1); vSplitter1->addWidget(hSplitter1); areaToPane[View::BOTTOM] = new TabWidget(this, vSplitter1); vSplitter1->addWidget(areaToPane[View::BOTTOM]); areaToPane[View::CENTER] = new TabWidget(this, hSplitter1); hSplitter1->addWidget(areaToPane[View::CENTER]); areaToPane[View::RIGHT] = new TabWidget(this, hSplitter1); hSplitter1->addWidget(areaToPane[View::RIGHT]); QList sizes; sizes << 100 << 600; topSplitter->setSizes(sizes); sizes.last() = 100; vSplitter0->setSizes(sizes); sizes.last() = 40; vSplitter1->setSizes(sizes); sizes.last() = 160; hSplitter1->setSizes(sizes); } #if 0 void MainWindowImpl::detectExistingPaneAreas() { for(int i=0; i < View::NUM_AREAS; ++i){ areaToPane[i] = 0; } vector infos; EdgeContactState edge; edge.set(); updateAreaDetectionInfos(topSplitter, edge, infos); if(infos.empty()){ createDefaultPanes(); } else { areaToPane[View::CENTER] = extractBestAreaMatchPane(infos, View::CENTER); areaToPane[View::LEFT] = extractBestAreaMatchPane(infos, View::LEFT); areaToPane[View::RIGHT] = extractBestAreaMatchPane(infos, View::RIGHT); areaToPane[View::BOTTOM] = extractBestAreaMatchPane(infos, View::BOTTOM); } } void MainWindowImpl::updateAreaDetectionInfos (QSplitter* splitter, const EdgeContactState& edge, vector& infos) { QWidget* childWidgets[2]; childWidgets[0] = splitter->get_child1(); childWidgets[1] = splitter->get_child2(); bool isSingle = !(childWidgets[0] && childWidgets[1]); for(int i=0; i < 2; ++i){ EdgeContactState currentEdge(edge); if(!isSingle){ if(dynamic_cast(splitter)){ currentEdge.reset((i == 0) ? BOTTOM : TOP); } else { currentEdge.reset((i == 0) ? RIGHT : LEFT); } } if(childWidgets[i]){ Splitter* childSplitter = dynamic_cast(childWidgets[i]); if(childSplitter){ updateAreaDetectionInfos(childSplitter, currentEdge, infos); } else { Pane* pane = dynamic_cast(childWidgets[i]); if(pane){ AreaDetectionInfo info; info.pane = pane; // calculate scores for area matching static const int offset = 100000000; int width = pane->get_width(); int height = pane->get_height(); info.scores[View::CENTER] = (4 - currentEdge.count()) * offset + width * height; if(currentEdge.test(LEFT) && !currentEdge.test(RIGHT)){ info.scores[View::LEFT] = offset + height; } if(currentEdge.test(RIGHT) && !currentEdge.test(LEFT)){ info.scores[View::RIGHT] = offset + height; } if(currentEdge.test(BOTTOM) && !currentEdge.test(TOP)){ info.scores[View::BOTTOM] = offset + width; } infos.push_back(info); } } } } } Pane* MainWindowImpl::extractBestAreaMatchPane(vector& infos, View::LayoutArea area) { int topScore = 0; int topIndex = -1; for(int i=0; i < (signed)infos.size(); ++i){ int s = infos[i].scores[area]; if(s > topScore){ topScore = s; topIndex = i; } } Pane* pane = 0; if(topIndex > 0){ pane = infos[topIndex].pane; infos.erase(infos.begin() + topIndex); } return pane; } #endif bool MainWindow::addView(const std::string& pluginName, View* view) { return impl->addView(pluginName, view); } bool MainWindowImpl::addView(const std::string& pluginName, View* view) { if(view->isManagedByMainWindow){ return false; } view->isManagedByMainWindow = true; areaToPane[view->defaultLayoutArea()]->addView(view); views.push_back(view); nameToViewMap.insert(make_pair(view->name(), view)); return true; } bool MainWindow::removeView(View* view) { return impl->removeView(view); } bool MainWindowImpl::removeView(View* view) { bool removed = false; if(view && view->isManagedByMainWindow){ std::remove(views.begin(), views.end(), view); TabWidget* tab = dynamic_cast(view->parentWidget()); if(tab){ tab->removeTab(tab->indexOf(view)); } view->isManagedByMainWindow = false; view->hide(); removed = true; } return removed; } void MainWindow::addToolBar(ToolBar* toolbar) { impl->toolBarArea->addToolBar(toolbar); } std::vector MainWindow::allViews() { return impl->views; } std::vector MainWindow::allToolBars() { return impl->toolBarArea->getAllToolBars(); } void MainWindow::setInitialLayout(const YamlMappingPtr layout) { if(TRACE_FUNCTIONS){ cout << "MainWindow::setInitialLayout()" << endl; } if(impl->isBeforeDoingInitialLayout){ impl->initialLayout = layout; impl->toolBarArea->setInitialLayout(layout); } } void MainWindow::changeEvent(QEvent* event) { if(event->type() == QEvent::WindowStateChange){ if(TRACE_FUNCTIONS){ cout << "MainWindow::changeEvent() of WindowStateChange: " << windowState() << endl; } if(!(windowState() & Qt::WindowFullScreen)){ impl->isMaximized = (windowState() & Qt::WindowMaximized); } } } void MainWindow::show() { impl->showFirst(); } void MainWindowImpl::showFirst() { if(TRACE_FUNCTIONS){ cout << "MainWindowImpl::showFirst()" << endl; } if(isBeforeShowing){ //self->resize(normalStateSize); if(config->get("fullScreen", false)){ isMaximizedJustBeforeFullScreen = isMaximized; self->showFullScreen(); } else if(config->get("maximized", true)){ self->showMaximized(); } else { self->QMainWindow::show(); } isBeforeShowing = false; } else { self->QMainWindow::show(); } } void MainWindowImpl::onFullScreenToggled(bool on) { if(on){ if(!self->isFullScreen()){ isMaximizedJustBeforeFullScreen = isMaximized; self->showFullScreen(); } } else { if(self->isFullScreen()){ if(isMaximizedJustBeforeFullScreen){ self->showMaximized(); } else { self->showNormal(); } } } } void MainWindow::resizeEvent(QResizeEvent* event) { QMainWindow::resizeEvent(event); impl->resizeEvent(event); } void MainWindowImpl::resizeEvent(QResizeEvent* event) { if(TRACE_FUNCTIONS){ cout << "MainWindowImpl::resizeEvent(): size = ("; cout << event->size().width() << ", " << event->size().height() << ")"; cout << ", isMaximized = " << (self->windowState() & Qt::WindowMaximized) << ", " << isMaximized; cout << ", isVisible = " << self->isVisible() << endl; } if(isBeforeDoingInitialLayout){ if(!(self->windowState() & (Qt::WindowMaximized | Qt::WindowFullScreen)) || self->isVisible()){ if(TRACE_FUNCTIONS){ cout << "MainWindowImpl::resizeEvent(): initializeLayout" << endl; } if(initialLayout){ restoreLayout(initialLayout); } else { restoreDefaultLayout(); } initialLayout = 0; isBeforeDoingInitialLayout = false; } } else { if(!(self->windowState() & (Qt::WindowMinimized | Qt::WindowMaximized | Qt::WindowFullScreen))){ // normal state ? normalStateSize = self->size(); } } } void MainWindow::keyPressEvent(QKeyEvent* event) { impl->keyPressEvent(event); } void MainWindowImpl::keyPressEvent(QKeyEvent* event) { switch(event->key()){ case Qt::Key_F11: fullScreenCheck->toggle(); break; default: break; } } void MainWindowImpl::restoreDefaultLayout() { /* detectExistingPaneAreas(); for(ViewSet::iterator p = storedViews.begin(); p != storedViews.end(); ++p){ View* view = *p; Pane* pane = areaToPane[view->defaultLayoutArea()]; if(!pane){ pane = areaToPane[View::CENTER]; } pane->appendView(view); } storedViews.clear(); topSplitter->show_all(); */ } void MainWindow::restoreLayout(const YamlMappingPtr layout) { impl->restoreLayout(layout); } void MainWindowImpl::onLoadLayout() { restoreLayout(config); } void MainWindowImpl::onLoadLayoutAs() { QFileDialog dialog(self); dialog.setWindowTitle(_("Open a layout")); dialog.setFileMode(QFileDialog::ExistingFile); dialog.setViewMode(QFileDialog::List); dialog.setLabelText(QFileDialog::Accept, _("Open")); dialog.setLabelText(QFileDialog::Reject, _("Cancel")); QStringList filters; filters << _("Layout files (*.conf)"); filters << _("Any files (*)"); dialog.setNameFilters(filters); if(!currentLayoutFolder.isEmpty()){ dialog.setDirectory(currentLayoutFolder); } if(dialog.exec()){ currentLayoutFolder = dialog.directory().absolutePath(); AppConfig::load(dialog.selectedFiles().front().toStdString()); config = AppConfig::archive()->openMapping("MainWindow"); restoreLayout(config); } } void MainWindowImpl::restoreLayout(const YamlMappingPtr layout) { if(TRACE_FUNCTIONS){ cout << "MainWindowImpl::restoreLayout()" << endl; } /* if(isBeforeDoingInitialLayout){ self->setInitialLayout(layout); return; } */ if(!isBeforeDoingInitialLayout){ toolBarArea->restoreLayout(layout); } else { toolBarArea->doInitialLayout(); } const YamlMappingPtr layoutOfViews = layout->findMapping("layoutOfViews"); if(layoutOfViews->isValid()){ clearAllPanes(); TabWidget* firstPane = 0; QWidget* restoredWidget = restoreSplitterState(*layoutOfViews, firstPane); topSplitter = dynamic_cast(restoredWidget); if(!topSplitter){ topSplitter = new QSplitter(); firstPane = dynamic_cast(restoredWidget); if(!firstPane){ firstPane = new TabWidget(this); } topSplitter->addWidget(firstPane); } centralVBox->addWidget(topSplitter, 1); for(ViewSet::iterator p = storedViews.begin(); p != storedViews.end(); ++p){ firstPane->addView(*p); } storedViews.clear(); } } void MainWindowImpl::clearAllPanes() { if(TRACE_FUNCTIONS){ cout << "clearAllPanes" << endl; } clearAllPanesSub(topSplitter); delete topSplitter; topSplitter = 0; if(TRACE_FUNCTIONS){ cout << "End of clearAllPanes" << endl; } } void MainWindowImpl::clearAllPanesSub(QSplitter* splitter) { if(TRACE_FUNCTIONS){ cout << "clearAllPanesSub" << endl; } for(int i=0; i < splitter->count(); ++i){ QSplitter* childSplitter = dynamic_cast(splitter->widget(i)); if(childSplitter){ clearAllPanesSub(childSplitter); } else { TabWidget* pane = dynamic_cast(splitter->widget(i)); if(pane){ for(int i=0; i < pane->count(); ++i){ View* view = dynamic_cast(pane->widget(i)); if(view){ storedViews.insert(view); } } while(pane->count() > 0){ int index = pane->count() - 1; QWidget* view = pane->widget(index); pane->removeTab(index); view->hide(); view->setParent(centralWidget); } } } } } QWidget* MainWindowImpl::restoreSplitterState(const YamlMapping& state, TabWidget*& out_firstPane) { if(TRACE_FUNCTIONS){ cout << "MainWindowImpl::restoreSplitterState" << endl; } QWidget* restoredWidget = 0; QWidget* childWidgets[2] = { 0, 0 }; const YamlSequence& children = *state.findSequence("children"); if(children.isValid()){ int numChildren = std::min(children.size(), 2); for(int i=0; i < numChildren; ++i){ if(children[i].isMapping()){ const YamlMapping& childState = *children[i].toMapping(); string type; if(childState.read("type", type)){ if(type == "splitter"){ childWidgets[i] = restoreSplitterState(childState, out_firstPane); } else if(type == "pane"){ TabWidget* pane = restorePaneState(childState); if(pane){ childWidgets[i] = pane; if(!out_firstPane){ out_firstPane = pane; } } } } } } if(childWidgets[0] && childWidgets[1]){ QSplitter* splitter = new QSplitter(); string orientation; if(state.read("orientation", orientation)){ splitter->setOrientation((orientation == "vertical") ? Qt::Vertical : Qt::Horizontal); } splitter->addWidget(childWidgets[0]); splitter->addWidget(childWidgets[1]); const YamlSequence& sizes = *state.findSequence("sizes"); if(sizes.isValid() && sizes.size() == 2){ QList s; int size; for(int i=0; i < 2; ++i){ if(sizes[i].read(size)){ s.push_back(size); } } splitter->setSizes(s); } restoredWidget = splitter; } else { for(int i=0; i < 2; ++i){ if(childWidgets[i]){ restoredWidget = childWidgets[i]; break; } } } } return restoredWidget; } TabWidget* MainWindowImpl::restorePaneState(const YamlMapping& state) { if(TRACE_FUNCTIONS){ cout << "MainWindowImpl::restorePaneState" << endl; } TabWidget* pane = 0; const YamlSequence& viewNames = *state.findSequence("views"); if(viewNames.isValid() && !viewNames.empty()){ pane = new TabWidget(this); QString currentViewName(state.get("current", "").c_str()); for(int i=0; i < viewNames.size(); ++i){ if(viewNames[i].isString()){ NameToViewMap::iterator p, upper_bound; tie(p, upper_bound) = nameToViewMap.equal_range(viewNames[i].toString().c_str()); if(p != upper_bound){ View* view = p->second; ViewSet::iterator q = storedViews.find(view); if(q != storedViews.end()){ storedViews.erase(q); pane->addView(view); if(view->name() == currentViewName){ pane->setCurrentIndex(i); } } } } } if(pane->count() == 0){ delete pane; pane = 0; } } return pane; } void MainWindow::storeLayout(YamlMappingPtr layout) { impl->storeLayout(layout); AppConfig::flush(); } void MainWindowImpl::onSaveLayout() { storeLayout(config); } void MainWindowImpl::onSaveLayoutAs() { QFileDialog dialog(self); dialog.setWindowTitle(_("Save a layout")); dialog.setFileMode(QFileDialog::AnyFile); dialog.setViewMode(QFileDialog::List); dialog.setLabelText(QFileDialog::Accept, _("Save")); dialog.setLabelText(QFileDialog::Reject, _("Cancel")); QStringList filters; filters << _("Layout files (*.conf)"); filters << _("Any files (*)"); dialog.setNameFilters(filters); if(!currentLayoutFolder.isEmpty()){ dialog.setDirectory(currentLayoutFolder); } if(dialog.exec()){ currentLayoutFolder = dialog.directory().absolutePath(); string filename(dialog.selectedFiles().front().toStdString()); string ext = filesystem::extension(filesystem::path(filename)); if(ext != ".conf"){ filename += ".conf"; } storeLayout(config); AppConfig::save(filename); } } void MainWindowImpl::storeLayout(YamlMappingPtr layout) { try { layout->insert("layoutOfViews", storeSplitterState(topSplitter)); toolBarArea->storeLayout(layout); } catch(const YamlNode::Exception& ex){ cout << ex.message() << endl; } } YamlMapping* MainWindowImpl::storeSplitterState(QSplitter* splitter) { YamlMapping* state = new YamlMapping(); state->write("type", "splitter"); if(splitter->count() == 2){ state->write("orientation", (splitter->orientation() == Qt::Vertical) ? "vertical" : "horizontal"); YamlSequence* sizeSeq = state->createSequence("sizes"); QList sizes = splitter->sizes(); for(int i=0; i < sizes.size(); ++i){ sizeSeq->append(sizes[i]); } } YamlSequence* children = state->createSequence("children"); for(int i=0; i < splitter->count(); ++i){ QSplitter* childSplitter = dynamic_cast(splitter->widget(i)); if(childSplitter){ children->append(storeSplitterState(childSplitter)); } else { TabWidget* pane = dynamic_cast(splitter->widget(i)); if(pane && pane->count() > 0){ children->append(storePaneState(pane)); } } } return state; } YamlMapping* MainWindowImpl::storePaneState(TabWidget* pane) { YamlMapping* state = new YamlMapping(); state->write("type", "pane"); YamlSequence* views = state->createFlowStyleSequence("views"); const int n = pane->count(); for(int i=0; i < n; ++i){ View* view = dynamic_cast(pane->widget(i)); if(view){ const string name(view->name().toStdString()); views->append(name, YAML_DOUBLE_QUOTED); if(i == pane->currentIndex()){ state->write("current", name, YAML_DOUBLE_QUOTED); } } } return state; } void MainWindowImpl::clearEmptyPanes() { } bool MainWindowImpl::viewTabMousePressEvent(TabWidget* pane, QMouseEvent* event) { if(event->button() == Qt::LeftButton){ tabDragStartPosition = event->pos(); } return false; } bool MainWindowImpl::viewTabMouseMoveEvent(TabWidget* pane, QMouseEvent* event) { if(!isViewDragging){ if(event->buttons() & Qt::LeftButton){ if((event->pos() - tabDragStartPosition).manhattanLength() > QApplication::startDragDistance()){ if(!pane->tabBar()->geometry().contains(event->pos())){ View* view = dynamic_cast(pane->currentWidget()); if(view){ isViewDragging = true; dragSrcPane = pane; startViewDrag(view); } } } } } else { dragDestPane = 0; QWidget* pointed = topSplitter->childAt(topSplitter->mapFromGlobal(event->globalPos())); while(pointed){ dragDestPane = dynamic_cast(pointed); if(dragDestPane){ dragView(event); break; } pointed = pointed->parentWidget(); } if(!dragDestPane){ rubberBand->hide(); } } return false; } bool MainWindowImpl::viewTabMouseReleaseEvent(TabWidget* pane, QMouseEvent *event) { if(isViewDragging){ if(dragDestPane){ if(isViewDraggingOutsidePane){ dropViewOutsidePane(); } else { dropViewInsidePane(); } } QApplication::restoreOverrideCursor(); } isViewDragging = false; return false; } void MainWindowImpl::startViewDrag(View* view) { QApplication::setOverrideCursor(Qt::ClosedHandCursor); } void MainWindowImpl::dragView(QMouseEvent* event) { dropEdge = LEFT; QPoint p = topSplitter->mapFromGlobal(event->globalPos()); const int w = topSplitter->width(); const int h = topSplitter->height(); int distance[4]; distance[LEFT] = p.x(); distance[TOP] = p.y(); distance[RIGHT] = w - p.x(); distance[BOTTOM] = h - p.y(); for(int i=TOP; i <= BOTTOM; ++i){ if(distance[dropEdge] > distance[i]){ dropEdge = i; } } isViewDraggingOutsidePane = false; if(distance[dropEdge] < 8){ isViewDraggingOutsidePane = true; dragViewOutsidePane(); } else { dragViewInsidePane(dragDestPane->mapFromGlobal(event->globalPos())); } } void MainWindowImpl::dragViewInsidePane(const QPoint& posInDestPane) { dropEdge = LEFT; const int w = dragDestPane->width(); const int h = dragDestPane->height(); int distance[4]; distance[LEFT] = posInDestPane.x(); distance[TOP] = posInDestPane.y(); distance[RIGHT] = w - posInDestPane.x(); distance[BOTTOM] = h - posInDestPane.y(); for(int i=TOP; i <= BOTTOM; ++i){ if(distance[dropEdge] > distance[i]){ dropEdge = i; } } QRect r; if(SPLIT_DISTANCE_THRESHOLD < distance[dropEdge]){ r.setRect(0, 0, w, h); dropEdge = OVER; } else if(dropEdge == LEFT){ r.setRect(0, 0, w / 2, h); } else if(dropEdge == TOP){ r.setRect(0, 0, w, h /2); } else if(dropEdge == RIGHT){ r.setRect(w / 2, 0, w / 2, h); } else if(dropEdge == BOTTOM){ r.setRect(0, h / 2, w, h / 2); } r.translate(centralWidget->mapFromGlobal(dragDestPane->mapToGlobal(QPoint(0, 0)))); rubberBand->setGeometry(r); rubberBand->show(); } void MainWindowImpl::dropViewInsidePane() { View* view = static_cast(dragSrcPane->currentWidget()); if(dropEdge == OVER){ int index = dragDestPane->addView(view); dragDestPane->setCurrentIndex(index); } else { QSize destSize = dragDestPane->size(); QSplitter* parentSplitter = static_cast(dragDestPane->parentWidget()); QList parentSizes = parentSplitter->sizes(); QSplitter* newSplitter = new QSplitter(parentSplitter); parentSplitter->insertWidget(parentSplitter->indexOf(dragDestPane), newSplitter); TabWidget* newTabWidget = new TabWidget(this, newSplitter); if(dropEdge == LEFT){ newSplitter->setOrientation(Qt::Horizontal); newSplitter->addWidget(newTabWidget); newSplitter->addWidget(dragDestPane); } else if(dropEdge == RIGHT){ newSplitter->setOrientation(Qt::Horizontal); newSplitter->addWidget(dragDestPane); newSplitter->addWidget(newTabWidget); } else if(dropEdge == TOP){ newSplitter->setOrientation(Qt::Vertical); newSplitter->addWidget(newTabWidget); newSplitter->addWidget(dragDestPane); } else { newSplitter->setOrientation(Qt::Vertical); newSplitter->addWidget(dragDestPane); newSplitter->addWidget(newTabWidget); } newTabWidget->addView(view); int half; if(newSplitter->orientation() == Qt::Horizontal){ half = destSize.height() / 2; } else { half = destSize.width() / 2; } QList sizes; sizes << half << half; newSplitter->setSizes(sizes); parentSplitter->setSizes(parentSizes); } if(dragSrcPane->count() > 0){ dragSrcPane->setCurrentIndex(0); } removePaneIfEmpty(dragSrcPane); rubberBand->hide(); } void MainWindowImpl::dragViewOutsidePane() { QRect r; int w = topSplitter->width(); int h = topSplitter->height(); if(dropEdge == LEFT){ r.setRect(0, 0, w / 2, h); } else if(dropEdge == TOP){ r.setRect(0, 0, w, h /2); } else if(dropEdge == RIGHT){ r.setRect(w / 2, 0, w / 2, h); } else if(dropEdge == BOTTOM){ r.setRect(0, h / 2, w, h / 2); } r.translate(topSplitter->pos()); rubberBand->setGeometry(r); rubberBand->show(); } void MainWindowImpl::dropViewOutsidePane() { View* view = static_cast(dragSrcPane->currentWidget()); QSize size = topSplitter->size(); if(topSplitter->count() >= 2){ QSplitter* newTopSplitter = new QSplitter(centralWidget); newTopSplitter->addWidget(topSplitter); topSplitter = newTopSplitter; centralVBox->addWidget(topSplitter, 1); } TabWidget* newTabWidget = new TabWidget(this, topSplitter); if(dropEdge == LEFT){ topSplitter->setOrientation(Qt::Horizontal); topSplitter->insertWidget(0, newTabWidget); } else if(dropEdge == RIGHT){ topSplitter->setOrientation(Qt::Horizontal); topSplitter->addWidget(newTabWidget); } else if(dropEdge == TOP){ topSplitter->setOrientation(Qt::Vertical); topSplitter->insertWidget(0, newTabWidget); } else { topSplitter->setOrientation(Qt::Vertical); topSplitter->addWidget(newTabWidget); } newTabWidget->addView(view); int half; if(topSplitter->orientation() == Qt::Horizontal){ half = size.height() / 2; } else { half = size.width() / 2; } QList sizes; sizes << half << half; topSplitter->setSizes(sizes); removePaneIfEmpty(dragSrcPane); rubberBand->hide(); } void MainWindowImpl::removePaneIfEmpty(TabWidget* pane) { if(pane->count() == 0){ QSplitter* parentSplitter = dynamic_cast(pane->parentWidget()); pane->deleteLater(); if(parentSplitter){ if(parentSplitter->count() <= 1){ QSplitter* grandParentSplitter = dynamic_cast(parentSplitter->parentWidget()); if(grandParentSplitter){ if(parentSplitter->count() == 1){ int parentSplitterIndex = grandParentSplitter->indexOf(parentSplitter); grandParentSplitter->insertWidget(parentSplitterIndex, parentSplitter->widget(0)); } delete parentSplitter; } } } } } choreonoid-1.1.0+dfsg/src/Base/MainWindow.h000066400000000000000000000026521207742442300204610ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_MAIN_WINDOW_H_INCLUDED #define CNOID_GUIBASE_MAIN_WINDOW_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class View; class ToolBar; class MainWindowImpl; class ExtensionManager; /** @if jp メインウィンドウ。 @endif */ class CNOID_EXPORT MainWindow : public QMainWindow { public: static void initialize(const char* appName, ExtensionManager* ext); static MainWindow* instance(); void show(); bool addView(const std::string& pluginName, View* view); bool removeView(View* view); void addToolBar(ToolBar* toolbar); std::vector allViews(); std::vector allToolBars(); void storeLayout(YamlMappingPtr layout); void restoreLayout(const YamlMappingPtr layout); void setInitialLayout(const YamlMappingPtr layout); protected: virtual void changeEvent(QEvent* event); virtual void resizeEvent(QResizeEvent* event); virtual void keyPressEvent(QKeyEvent* event); private: MainWindowImpl* impl; MainWindow(const char* appName, ExtensionManager* ext); virtual ~MainWindow(); friend class AppImpl; friend class ExtensionManager; friend class View; }; } #endif choreonoid-1.1.0+dfsg/src/Base/MenuManager.cpp000066400000000000000000000200221207742442300211260ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "MenuManager.h" #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; Menu::Menu(QWidget* parent) : QMenu(parent) { initialize(); } Menu::Menu(const QString& title, QWidget* parent) : QMenu(title, parent) { initialize(); } void Menu::initialize() { connect(this, SIGNAL(triggered(QAction*)), this, SLOT(onTriggered(QAction*))); connect(this, SIGNAL(aboutToHide()), this, SLOT(onAboutToHide())); } void Menu::onTriggered(QAction* action) { sigTriggered_(action); } void Menu::onAboutToHide() { sigAboutToHide_(); } namespace cnoid { class MenuManagerImpl { public: MenuManager* self; QWidget* topMenu; QWidget* currentMenu; bool isBackwardMode; typedef set ActionSet; ActionSet allActions; std::string textDomain; MenuManagerImpl(MenuManager* self, QWidget* topMenu); ~MenuManagerImpl(); void setTopMenu(QWidget* topMenu); void releaseAllItems(); void setReference(QAction* item); void unsetReference(QAction* item); pair findPath(const QString& path, bool createPath); void addItem(QWidget* menu, QAction* item); void setPath(const QString& path); MenuManager& setBackwardMode(); Action* addItem(const QString& text); void addSeparator(); void removeItem(QAction* item); }; } MenuManager::MenuManager() { impl = new MenuManagerImpl(this, 0); } MenuManager::MenuManager(QWidget* topMenu) { impl = new MenuManagerImpl(this, topMenu); } MenuManagerImpl::MenuManagerImpl(MenuManager* self, QWidget* topMenu) : self(self) { this->topMenu = topMenu; currentMenu = topMenu; isBackwardMode = false; } void MenuManager::bindTextDomain(const std::string& domain) { impl->textDomain = domain; } void MenuManager::setTopMenu(QWidget* topMenu) { impl->setTopMenu(topMenu); } void MenuManagerImpl::setTopMenu(QWidget* topMenu) { if(this->topMenu){ releaseAllItems(); } this->topMenu = topMenu; currentMenu = topMenu; } MenuManager::~MenuManager() { delete impl; } MenuManagerImpl::~MenuManagerImpl() { releaseAllItems(); } void MenuManager::releaseAllItems() { impl->releaseAllItems(); } void MenuManagerImpl::releaseAllItems() { while(true){ ActionSet::iterator p = allActions.begin(); if(p == allActions.end()){ break; } QAction* action = *p; allActions.erase(p); unsetReference(action); } currentMenu = topMenu; isBackwardMode = false; } void MenuManagerImpl::setReference(QAction* item) { ActionSet::iterator p = allActions.find(item); if(p == allActions.end()){ allActions.insert(item); QObject::connect(item, SIGNAL(destroyed()), self, SLOT(onItemDestroyed())); int ref = 0; QVariant mmref = item->property("mmref"); if(mmref.isValid()){ ref = mmref.toInt(); } item->setProperty("mmref", ref + 1); } } void MenuManager::onItemDestroyed() { impl->allActions.erase(dynamic_cast(sender())); } void MenuManagerImpl::unsetReference(QAction* item) { if(QObject::disconnect(item, SIGNAL(destroyed()), self, SLOT(onItemDestroyed()))){ QVariant mmref = item->property("mmref"); if(mmref.isValid()){ int ref = mmref.toInt() - 1; if(ref == 0){ delete item; } else { item->setProperty("mmref", ref); } } } } QAction* MenuManager::findItem(const QString& path) { return impl->findPath(path, false).first; } pair MenuManagerImpl::findPath(const QString& path, bool createPath) { int pos = 0; int size = path.size(); QAction* item = 0; QWidget* menu = currentMenu; if(path[pos] == QChar('/')){ pos++; menu = topMenu; } while(menu && (pos != size)){ int next = path.indexOf(QChar('/'), pos); int length = (next >= 0) ? (next - pos) : next; QString name = path.mid(pos, length); QList items = menu->actions(); item = 0; for(int i=0; i < items.size(); ++i){ if(name == items[i]->objectName()){ item = items[i]; } } if(!item){ if(createPath){ if(textDomain.empty()){ item = new QAction(name, menu); } else { item = new QAction(dgettext(textDomain.c_str(), name.toAscii()), menu); } item->setObjectName(name); addItem(menu, item); item->setMenu(new QMenu()); } else { break; } } setReference(item); menu = item->menu(); pos = (next >= 0) ? (next + 1) : size; } return make_pair(item, menu); } void MenuManagerImpl::addItem(QWidget* menu, QAction* item) { QList items = menu->actions(); int position; for(position = items.size() - 1; position >= 0; --position){ QAction* sibling = items[position]; if(!sibling->property("isBackward").toBool()){ break; } } position++; if(position < items.size()){ menu->insertAction(items[position], item); } else { menu->addAction(item); } setReference(item); if(isBackwardMode){ item->setProperty("isBackward", true); } } /** @if jp アイテムæ“作ã®å¯¾è±¡ã¨ãªã‚‹ãƒ¡ãƒ‹ãƒ¥ãƒ¼ã‚’指定ã™ã‚‹ã€‚ メニューアイテムを追加ã™ã‚‹å‰ã«ã‚らã‹ã˜ã‚呼んã§ãŠãå¿…è¦ãŒã‚る。 @param menuPath 対象ã¨ãªã‚‹ãƒ¡ãƒ‹ãƒ¥ãƒ¼ã¸ã®ãƒ‘スを指定ã™ã‚‹ã€‚ '/' ã‹ã‚‰å§‹ã¾ã‚‹å ´åˆãƒ«ãƒ¼ãƒˆã‹ã‚‰ã®ä½ç½®ã«ãªã‚Šã€ãã†ã§ãªã„å ´åˆã¯ ç¾åœ¨æŒ‡å®šã•ã‚Œã¦ã„るメニューã‹ã‚‰ã®ç›¸å¯¾ãƒ‘スã«ãªã‚‹ã€‚ ã‚‚ã—パスã«æŒ‡å®šã•ã‚ŒãŸãƒ¡ãƒ‹ãƒ¥ãƒ¼ãŒå­˜åœ¨ã—ãªã„å ´åˆã¯ãƒ¡ãƒ‹ãƒ¥ãƒ¼ãŒæ–°ãŸã«ä½œæˆã•ã‚Œã‚‹ã€‚ @endif */ MenuManager& MenuManager::setPath(const QString& path) { impl->setPath(path); return *this; } void MenuManagerImpl::setPath(const QString& path) { if(!path.isEmpty() && path[0] == QChar('/')){ isBackwardMode = false; } QAction* item; QWidget* menu; tie(item, menu) = findPath(path, true); if(!menu){ cerr << "cnoid::MenuManager warning: setting path to " << path.toLocal8Bit().data() << " failed." << endl; } currentMenu = menu; isBackwardMode = false; } MenuManager& MenuManager::setBackwardMode() { impl->isBackwardMode = true; return *this; } Action* MenuManager::addItem(const QString& text) { return impl->addItem(text); } Action* MenuManagerImpl::addItem(const QString& text) { Action* item = new Action(text, currentMenu); addItem(currentMenu, item); return item; } Action* MenuManager::addCheckItem(const QString& text) { Action* item = impl->addItem(text); if(item){ item->setCheckable(true); } return item; } Action* MenuManager::addRadioItem(QActionGroup* group, const QString& text) { Action* item = impl->addItem(text); if(item){ item->setActionGroup(group); } return item; } MenuManager& MenuManager::addSeparator() { impl->addSeparator(); return *this; } /** \todo implement smart separator */ void MenuManagerImpl::addSeparator() { QAction* separator = new QAction(currentMenu); separator->setSeparator(true); addItem(currentMenu, separator); } void MenuManager::removeItem(QAction* item) { impl->removeItem(item); } void MenuManagerImpl::removeItem(QAction* item) { ActionSet::iterator p = allActions.find(item); if(p != allActions.end()){ allActions.erase(p); unsetReference(item); } } choreonoid-1.1.0+dfsg/src/Base/MenuManager.h000066400000000000000000000037151207742442300206050ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_MENU_MANAGER_H_INCLUDED #define CNOID_GUIBASE_MENU_MANAGER_H_INCLUDED #include "Action.h" #include #include #include #include "exportdecl.h" namespace cnoid { class MenuManagerImpl; class CNOID_EXPORT Menu : public QMenu { Q_OBJECT public: Menu(QWidget* parent = 0); Menu(const QString& title, QWidget* parent = 0); inline SignalProxy< boost::signal > sigTriggered() { return sigTriggered_; } inline SignalProxy< boost::signal > sigAboutToHide() { return sigAboutToHide_; } private Q_SLOTS: void onTriggered(QAction* action); void onAboutToHide(); private: boost::signal sigTriggered_; boost::signal sigAboutToHide_; void initialize(); }; /** @if jp メニューを簡å˜ã«ç®¡ç†ã™ã‚‹ãŸã‚ã®ã‚¯ãƒ©ã‚¹ã€‚ @endif */ class CNOID_EXPORT MenuManager : public QObject { Q_OBJECT public: MenuManager(); MenuManager(QWidget* topMenu); virtual ~MenuManager(); void bindTextDomain(const std::string& domain); void setTopMenu(QWidget* topMenu); QAction* findItem(const QString& path); MenuManager& setPath(const QString& path); MenuManager& setBackwardMode(); void addAction(QAction* action); Action* addItem(const QString& text); Action* addCheckItem(const QString& text); Action* addRadioItem(QActionGroup* group, const QString& text); MenuManager& addSeparator(); void removeItem(QAction* item); void releaseAllItems(); private: MenuManager(const MenuManager* org); MenuManagerImpl* impl; public Q_SLOTS: void onItemDestroyed(); }; } #endif choreonoid-1.1.0+dfsg/src/Base/MessageView.cpp000066400000000000000000000202561207742442300211570ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "MessageView.h" #include "MainWindow.h" #include "ExtensionManager.h" #include "InfoBar.h" #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { class TextSink : public iostreams::sink { public: TextSink(MessageViewImpl* messageViewImpl, bool doFlush); std::streamsize write(const char* s, std::streamsize n); MessageViewImpl* messageViewImpl; QPlainTextEdit& textEdit; bool doFlush; }; struct StdioInfo { streambuf* cout; streambuf* cerr; }; enum MvCommand { MV_PUT, MV_CLEAR }; class MessageViewEvent : public QEvent { public: MessageViewEvent(const QString& message, bool doLF, bool doNotify, bool doFlush) : QEvent(QEvent::User), command(MV_PUT), message(message), doLF(doLF), doNotify(doNotify), doFlush(doFlush) { } MessageViewEvent(MvCommand command) : QEvent(QEvent::User), command(command) { } MvCommand command; QString message; bool doLF; bool doNotify; bool doFlush; }; } namespace cnoid { class MessageViewImpl { public: MessageView* self; Qt::HANDLE mainThreadId; QPlainTextEdit textEdit; TextSink textSink; iostreams::stream_buffer sbuf; std::ostream os; TextSink textSink_flush; iostreams::stream_buffer sbuf_flush; std::ostream os_flush; std::stack stdios; bool exitEventLoopRequested; MessageViewImpl(MessageView* self); void doPut(const QString& message, bool doLF, bool doNotify, bool doFlush) { textEdit.moveCursor(QTextCursor::End); if(!doLF){ textEdit.insertPlainText(message); } else { textEdit.insertPlainText(message + "\n"); } textEdit.moveCursor(QTextCursor::End); if(doNotify){ InfoBar::instance()->notify(message); } if(doFlush){ flush(); } } void put(const QString& message, bool doLF, bool doNotify, bool doFlush) { if(QThread::currentThreadId() == mainThreadId){ doPut(message, doLF, doNotify, doFlush); } else { MessageViewEvent* event = new MessageViewEvent(message, doLF, doNotify, doFlush); QCoreApplication::postEvent(self, event, Qt::NormalEventPriority); } } void handleMessageViewEvent(MessageViewEvent* event); void flush(); void doClear(){ textEdit.clear(); } void clear() { if(QThread::currentThreadId() == mainThreadId){ doClear(); } else { QCoreApplication::postEvent(self, new MessageViewEvent(MV_CLEAR), Qt::NormalEventPriority); } } }; } TextSink::TextSink(MessageViewImpl* messageViewImpl, bool doFlush) : messageViewImpl(messageViewImpl), textEdit(messageViewImpl->textEdit), doFlush(doFlush) { } std::streamsize TextSink::write(const char* s, std::streamsize n) { messageViewImpl->put(QString::fromLocal8Bit(s, n), false, false, doFlush); //messageViewImpl->put(QString::fromUtf8(s, n), false, false, doFlush); return n; } void MessageView::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ext->addView(mainInstance()); initialized = true; } } MessageView* MessageView::mainInstance() { static MessageView* messageView = new MessageView(); return messageView; } MessageView::MessageView() { impl = new MessageViewImpl(this); } MessageViewImpl::MessageViewImpl(MessageView* self) : self(self), mainThreadId(QThread::currentThreadId()), textSink(this, false), sbuf(textSink), os(&sbuf), textSink_flush(this, true), sbuf_flush(textSink_flush), os_flush(&sbuf_flush) { self->setName(N_("Message")); self->setDefaultLayoutArea(View::BOTTOM); textEdit.setFrameShape(QFrame::NoFrame); textEdit.setReadOnly(true); textEdit.setWordWrapMode(QTextOption::WrapAnywhere); QHBoxLayout* layout = new QHBoxLayout(); layout->addWidget(&textEdit); self->setLayout(layout); } MessageView::~MessageView() { delete impl; } bool MessageView::event(QEvent* e) { MessageViewEvent* event = dynamic_cast(e); if(event){ impl->handleMessageViewEvent(event); return true; } return false; } void MessageViewImpl::handleMessageViewEvent(MessageViewEvent* event) { switch(event->command){ case MV_PUT: doPut(event->message, event->doLF, event->doNotify, event->doFlush); break; case MV_CLEAR: doClear(); break; default: break; } } void MessageView::put(const std::string& message) { impl->put(message.c_str(), false, false, false); } void MessageView::put(const boost::format& message) { impl->put(message.str().c_str(), false, false, false); } void MessageView::put(const char* message) { impl->put(message, false, false, false); } void MessageView::put(const QString& message) { impl->put(message, false, false, false); } void MessageView::putln() { impl->put(QString(), true, false, false); } void MessageView::putln(const std::string& message) { impl->put(message.c_str(), true, false, false); } void MessageView::putln(const boost::format& message) { impl->put(message.str().c_str(), true, false, false); } void MessageView::putln(const char* message) { impl->put(message, true, false, false); } void MessageView::putln(const QString& message) { impl->put(message, true, false, false); } void MessageView::notify(const std::string& message) { impl->put(message.c_str(), true, true, false); } void MessageView::notify(const boost::format& message) { impl->put(message.str().c_str(), true, true, false); } void MessageView::notify(const char* message) { impl->put(message, true, true, false); } void MessageView::notify(const QString& message) { impl->put(message, true, true, false); } /** @note Don't call this function from an expose event handler because it may cause a hangup of rendering. */ void MessageView::flush() { impl->flush(); } void MessageViewImpl::flush() { if(QThread::currentThreadId() == mainThreadId){ #ifdef Q_OS_WIN32 QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents); #else //QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents); while(QCoreApplication::hasPendingEvents()){ QCoreApplication::processEvents(); } #endif } } void MessageView::clear() { impl->clear(); } std::ostream& MessageView::cout(bool doFlush) { return doFlush ? impl->os_flush : impl->os; } void MessageView::beginStdioRedirect() { StdioInfo info; info.cout = std::cout.rdbuf(); info.cerr = std::cerr.rdbuf(); impl->stdios.push(info); std::cout.rdbuf(impl->os.rdbuf()); std::cerr.rdbuf(impl->os.rdbuf()); } void MessageView::endStdioRedirect() { if(!impl->stdios.empty()){ StdioInfo& info = impl->stdios.top(); std::cout.rdbuf(info.cout); std::cerr.rdbuf(info.cerr); impl->stdios.pop(); } } void cnoid::showWarningDialog(const QString& message) { QMessageBox::warning(MainWindow::instance(), _("Warning"), message); } void cnoid::showWarningDialog(const char* message) { showWarningDialog(QString(message)); } void cnoid::showWarningDialog(const std::string& message) { showWarningDialog(message.c_str()); } void cnoid::showWarningDialog(const boost::format& message) { showWarningDialog(message.str().c_str()); } choreonoid-1.1.0+dfsg/src/Base/MessageView.h000066400000000000000000000032401207742442300206160ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_MESSAGE_VIEW_H_INCLUDED #define CNOID_GUIBASE_MESSAGE_VIEW_H_INCLUDED #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class ExtensionManager; class MessageViewImpl; class CNOID_EXPORT MessageView : public View { public: static void initialize(ExtensionManager* ext); static MessageView* mainInstance(); MessageView(); ~MessageView(); void put(const std::string& message); void put(const boost::format& message); void putln(const std::string& message); void putln(const boost::format& message); void notify(const std::string& message); void notify(const boost::format& message); void put(const char* message); void put(const QString& message); void putln(); void putln(const char* message); void putln(const QString& message); void notify(const char* message); void notify(const QString& message); void flush(); void clear(); std::ostream& cout(bool doFlush = false); void beginStdioRedirect(); void endStdioRedirect(); protected: virtual bool event(QEvent* e); private: MessageViewImpl* impl; }; CNOID_EXPORT void showWarningDialog(const std::string& message); CNOID_EXPORT void showWarningDialog(const boost::format& message); CNOID_EXPORT void showWarningDialog(const char* message); CNOID_EXPORT void showWarningDialog(const QString& message); } #endif choreonoid-1.1.0+dfsg/src/Base/MovieGenerator.cpp000066400000000000000000000170201207742442300216610ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "MovieGenerator.h" #include "TimeBar.h" #include "SceneView.h" #include "ExtensionManager.h" #include #include "Spacer.h" #include "Archive.h" #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace Glib; using namespace boost; using namespace cnoid; namespace { class MovieGenerator : public Gtk::Dialog { public: MovieGenerator(); ~MovieGenerator(); bool store(Archive& archive); void restore(const Archive& archive); protected: virtual void on_response(int id); private: bool isRecording; bool requestStopRecording; Gtk::Entry folderEntry; Gtk::Button folderButton; Gtk::Entry basenameEntry; Gtk::SpinButton beginningTimeSpin; Gtk::CheckButton endingTimeCheck; Gtk::SpinButton endingTimeSpin; Gtk::SpinButton fpsSpin; Gtk::SpinButton imageWidthSpin; Gtk::SpinButton imageHeightSpin; bool doRecordingLoop(); }; } void cnoid::initializeMovieGenerator(ExtensionManager& ext) { MovieGenerator* mg = ext.manage(new MovieGenerator()); MenuManager& mm = ext.menuManager(); mm.setPath("/Tools"); mm.addItem(N_("Movie Generator")) ->signal_activate().connect(sigc::mem_fun(*mg, &MovieGenerator::show)); ext.connectProjectArchiver( "MovieGenerator", bind(&MovieGenerator::store, mg, _1), bind(&MovieGenerator::restore, mg, _1)); } MovieGenerator::MovieGenerator() : Gtk::Dialog(_("Movie Generator"), MainWindow::instance()) { set_modal(false); isRecording = false; requestStopRecording = false; Gtk::VBox* vbox = get_vbox(); Gtk::HBox* hbox = Gtk::manage(new Gtk::HBox()); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("Folder"))), Gtk::PACK_SHRINK); hbox->pack_start(folderEntry, Gtk::PACK_SHRINK); folderEntry.set_width_chars(20); folderEntry.set_editable(); folderButton.set_label(_("Open")); hbox->pack_start(folderButton, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("Basename"))), Gtk::PACK_SHRINK); basenameEntry.set_text("scene"); hbox->pack_start(basenameEntry, Gtk::PACK_SHRINK); basenameEntry.set_width_chars(20); basenameEntry.set_editable(); hbox = Gtk::manage(new Gtk::HBox()); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("Begin"))), Gtk::PACK_SHRINK); beginningTimeSpin.set_digits(1); beginningTimeSpin.set_range(0.0, 9999.9); beginningTimeSpin.set_value(0.0); beginningTimeSpin.set_increments(0.1, 1.0); hbox->pack_start(beginningTimeSpin, Gtk::PACK_SHRINK); endingTimeCheck.set_label(_("End")); hbox->pack_start(endingTimeCheck, Gtk::PACK_SHRINK); endingTimeSpin.set_digits(1); endingTimeSpin.set_range(0.0, 9999.9); endingTimeSpin.set_value(0.0); endingTimeSpin.set_increments(0.1, 1.0); hbox->pack_start(endingTimeSpin, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label("FPS")), Gtk::PACK_SHRINK); fpsSpin.set_digits(1); fpsSpin.set_range(1.0, 9999.9); fpsSpin.set_value(30.0); fpsSpin.set_increments(0.1, 1.0); hbox->pack_start(fpsSpin, Gtk::PACK_SHRINK); hbox = Gtk::manage(new Gtk::HBox()); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("Image size"))), Gtk::PACK_SHRINK); imageWidthSpin.set_digits(0); imageWidthSpin.set_range(1, 9999); imageWidthSpin.set_value(640); imageWidthSpin.set_increments(1, 10); hbox->pack_start(imageWidthSpin, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label("x")), Gtk::PACK_SHRINK); imageHeightSpin.set_digits(0); imageHeightSpin.set_range(1, 9999); imageHeightSpin.set_value(480); imageHeightSpin.set_increments(1, 10); hbox->pack_start(imageHeightSpin, Gtk::PACK_SHRINK); show_all_children(); add_button(_("Generate"), Gtk::RESPONSE_OK); add_button(_("Cancel / Stop"), Gtk::RESPONSE_CANCEL); } MovieGenerator::~MovieGenerator() { } bool MovieGenerator::store(Archive& archive) { archive.write("folder", folderEntry.get_text()); archive.write("basename", basenameEntry.get_text()); archive.write("begin", beginningTimeSpin.get_value()); if(endingTimeCheck.get_active()){ archive.write("end", endingTimeSpin.get_value()); } archive.write("fps", fpsSpin.get_value()); archive.write("width", imageWidthSpin.get_value()); archive.write("heiht", imageHeightSpin.get_value()); return true; } void MovieGenerator::restore(const Archive& archive) { folderEntry.set_text(archive.get("folder", folderEntry.get_text().raw())); basenameEntry.set_text(archive.get("basename", basenameEntry.get_text().raw())); beginningTimeSpin.set_value(archive.get("begin", beginningTimeSpin.get_value())); double endingTime; if(archive.read("end", endingTime)){ endingTimeSpin.set_value(endingTime); endingTimeCheck.set_active(true); } else { endingTimeCheck.set_active(false); } fpsSpin.set_value(archive.get("fps", fpsSpin.get_value())); imageWidthSpin.set_value(archive.get("width", imageWidthSpin.get_value())); imageHeightSpin.set_value(archive.get("height", imageHeightSpin.get_value())); } void MovieGenerator::on_response(int id) { bool processed = false; if(id == Gtk::RESPONSE_OK){ processed = doRecordingLoop(); if(processed){ hide(); } } else { if(isRecording){ requestStopRecording = true; } else { hide(); } } } bool MovieGenerator::doRecordingLoop() { requestStopRecording = false; isRecording = true; int frame = 0; double time = beginningTimeSpin.get_value(); double endingTime = endingTimeCheck.get_active() ? endingTimeSpin.get_value() : std::numeric_limits::max(); double timeStep = 1.0 / fpsSpin.get_value(); TimeBar* timeBar = TimeBar::instance(); SceneView* sceneView = SceneView::mainInstance(); bool doContinue = true; filesystem::path folder(folderEntry.get_text()); filesystem::path basename(basenameEntry.get_text() + "%08u.png"); if(!folder.empty()){ if(filesystem::exists(folder)){ if(!is_directory(folder)){ Gtk::MessageDialog dialog(str(format(_("%1% is not a directory.")) % folder), false, Gtk::MESSAGE_ERROR); dialog.run(); return false; } } else { create_directories(folder); } } format filenameFormat((folder / basename).file_string()); sceneView->setScreenSize(imageWidthSpin.get_value_as_int(), imageHeightSpin.get_value_as_int()); while(time < endingTime && doContinue){ doContinue = timeBar->setTime(time); while(Glib::MainContext::get_default()->iteration(false)) { }; if(requestStopRecording){ break; } sceneView->saveImage(str(filenameFormat % frame)); time += timeStep; frame++; } isRecording = false; return !requestStopRecording; } choreonoid-1.1.0+dfsg/src/Base/MovieGenerator.h000066400000000000000000000004071207742442300213270ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_MOVIE_GENERATOR_H_INCLUDED #define CNOID_GUIBASE_MOVIE_GENERATOR_H_INCLUDED namespace cnoid { class ExtensionManager; void initializeMovieGenerator(ExtensionManager& ext); } #endif choreonoid-1.1.0+dfsg/src/Base/MultiAffine3SeqItem.cpp000066400000000000000000000011031207742442300225040ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "MultiAffine3SeqItem.h" #include "MultiSeqItemCreationPanel.h" #include "ItemManager.h" #include "gettext.h" using namespace cnoid; template<> void MultiSeqItem::initialize(ExtensionManager* ext) { ext->itemManager().registerClass(N_("MultiAffine3SeqItem")); ext->itemManager().addCreationPanel( new MultiSeqItemCreationPanel(_("Number of Affine3 values in a frame"))); } #ifdef WIN32 template class MultiSeqItem; #endif choreonoid-1.1.0+dfsg/src/Base/MultiAffine3SeqItem.h000066400000000000000000000007111207742442300221550ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_MULTI_AFFINE3_SEQ_ITEM_H_INCLUDED #define CNOID_GUIBASE_MULTI_AFFINE3_SEQ_ITEM_H_INCLUDED #include "MultiSeqItem.h" #include namespace cnoid { typedef MultiSeqItem MultiAffine3SeqItem; typedef MultiAffine3SeqItem::Ptr MultiAffine3SeqItemPtr; template<> void MultiSeqItem::initialize(ExtensionManager* ext); } #endif choreonoid-1.1.0+dfsg/src/Base/MultiSeqItem.cpp000066400000000000000000000035261207742442300213230ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "MultiSeqItem.h" #include "Archive.h" #include "PutPropertyFunction.h" #include #include "gettext.h" using namespace boost; using namespace cnoid; MultiSeqItemBase::MultiSeqItemBase() { } MultiSeqItemBase::MultiSeqItemBase(const MultiSeqItemBase& org) : Item(org) { } MultiSeqItemBase::~MultiSeqItemBase() { } void MultiSeqItemBase::notifyUpdate() { setInconsistencyWithLastAccessedFile(); Item::notifyUpdate(); } static bool setPropertyNumFrames(MultiSeqBase* seq, int numFrames) { if(numFrames >= 0){ seq->setNumFrames(numFrames); return true; } return false; } static bool setPropertyTimeLength(MultiSeqBase* seq, double timeLength) { if(timeLength >= 0){ seq->setTimeLength(timeLength); return true; } return false; } void MultiSeqItemBase::doPutPropertiesSub(PutPropertyFunction& putProperty, MultiSeqBase* seq) { putProperty(_("Frame rate"), seq->getFrameRate()); putProperty(_("Num parts"), seq->getNumParts()); putProperty(_("Num frames"), seq->getNumFrames(), bind(setPropertyNumFrames, seq, _1)); putProperty(_("Time length"), seq->getTimeLength(), bind(setPropertyTimeLength, seq, _1)); putProperty(_("Time step"), seq->getTimeStep()); } bool MultiSeqItemBase::storeSub(Archive& archive) { if(overwrite()){ archive.writeRelocatablePath("filename", lastAccessedFileName()); archive.write("format", lastAccessedFileFormatId()); return true; } return false; } bool MultiSeqItemBase::restoreSub(const Archive& archive) { std::string filename, formatId; if(archive.readRelocatablePath("filename", filename) && archive.read("format", formatId)){ if(load(filename, formatId)){ return true; } } return false; } choreonoid-1.1.0+dfsg/src/Base/MultiSeqItem.h000066400000000000000000000040731207742442300207660ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_MULTI_SEQ_ITEM_H_INCLUDED #define CNOID_GUIBASE_MULTI_SEQ_ITEM_H_INCLUDED #include "Item.h" #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT MultiSeqItemBase : public Item { public: MultiSeqItemBase(); MultiSeqItemBase(const MultiSeqItemBase& org); virtual ~MultiSeqItemBase(); virtual MultiSeqBasePtr seqBase() = 0; virtual void notifyUpdate(); protected: void doPutPropertiesSub(PutPropertyFunction& putProperty, MultiSeqBase* seq); bool storeSub(Archive& archive); bool restoreSub(const Archive& archive); }; template class MultiSeqItem : public MultiSeqItemBase { public: static void initialize(ExtensionManager* ext) { } typedef boost::intrusive_ptr< MultiSeqItem > Ptr; MultiSeqItem() : seq_(new MultiSeqType()) { } MultiSeqItem(typename MultiSeqType::Ptr seq) : seq_(seq) { } virtual MultiSeqBasePtr seqBase() { return seq_; } inline boost::shared_ptr seq() { return seq_; } //void resetSeq(boost::shared_ptr seq) { seq_ = seq; } protected: MultiSeqItem(const MultiSeqItem& org) : MultiSeqItemBase(org), seq_(new MultiSeqType(*org.seq_)) { } virtual ItemPtr doDuplicate() const { return new MultiSeqItem(*this); } virtual void doPutProperties(PutPropertyFunction& putProperty) { doPutPropertiesSub(putProperty, seq_.get()); } virtual bool store(Archive& archive) { return storeSub(archive); } virtual bool restore(const Archive& archive) { return restoreSub(archive); } private: virtual ~MultiSeqItem() { } boost::shared_ptr seq_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/MultiSeqItemCreationPanel.cpp000066400000000000000000000044731207742442300237720ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "MultiSeqItemCreationPanel.h" #include "MultiSeqItem.h" #include #include #include "gettext.h" using namespace std; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } MultiSeqItemCreationPanel::MultiSeqItemCreationPanel(const QString& numSeqsCaption) { QVBoxLayout* vbox = new QVBoxLayout(); setLayout(vbox); QHBoxLayout* hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(_("Name"))); nameEntry = new QLineEdit(); hbox->addWidget(nameEntry); vbox->addLayout(hbox); hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(numSeqsCaption)); numSeqsSpin = new SpinBox(); numSeqsSpin->setRange(1, 999); hbox->addWidget(numSeqsSpin); hbox->addWidget(new QLabel(_("Time length"))); timeLengthSpin = new DoubleSpinBox(); timeLengthSpin->setDecimals(2); timeLengthSpin->setRange(0.0, 9999.99); timeLengthSpin->setSingleStep(0.01); hbox->addWidget(timeLengthSpin); hbox->addWidget(new QLabel(_("Frame rate"))); frameRateSpin = new DoubleSpinBox(); frameRateSpin->setDecimals(0); frameRateSpin->setRange(1.0, 9999.0); hbox->addWidget(frameRateSpin); vbox->addLayout(hbox); } bool MultiSeqItemCreationPanel::initializePanel(Item* protoItem) { nameEntry->setText(protoItem->name().c_str()); MultiSeqItemBase* item = dynamic_cast(protoItem); if(item){ MultiSeqBasePtr seq = item->seqBase(); numSeqsSpin->setValue(seq->getNumParts()); double frameRate = seq->getFrameRate(); timeLengthSpin->setValue(seq->getNumFrames() / frameRate); frameRateSpin->setValue(frameRate); return true; } return false; } bool MultiSeqItemCreationPanel::initializeItem(Item* protoItem) { protoItem->setName(nameEntry->text().toStdString()); MultiSeqItemBase* item = dynamic_cast(protoItem); if(item){ MultiSeqBasePtr seq = item->seqBase(); double frameRate = frameRateSpin->value(); seq->setFrameRate(frameRate); seq->setNumParts(numSeqsSpin->value()); seq->setNumFrames(static_cast(timeLengthSpin->value() * frameRate)); return true; } return false; } choreonoid-1.1.0+dfsg/src/Base/MultiSeqItemCreationPanel.h000066400000000000000000000013511207742442300234270ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_MULTI_SEQ_ITEM_CREATION_PANEL_H_INCLUDED #define CNOID_GUIBASE_MULTI_SEQ_ITEM_CREATION_PANEL_H_INCLUDED #include "SpinBox.h" #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT MultiSeqItemCreationPanel : public ItemCreationPanel { public: MultiSeqItemCreationPanel(const QString& numSeqsCaption); virtual bool initializePanel(Item* protoItem); virtual bool initializeItem(Item* protoItem); private: QLineEdit* nameEntry; SpinBox* numSeqsSpin; DoubleSpinBox* timeLengthSpin; DoubleSpinBox* frameRateSpin; }; } #endif choreonoid-1.1.0+dfsg/src/Base/MultiValueSeqItem.cpp000066400000000000000000000025571207742442300223230ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "MultiValueSeqItem.h" #include "MultiSeqItemCreationPanel.h" #include "ItemManager.h" #include #include "gettext.h" using namespace cnoid; static bool loadPlainSeqFormat(MultiValueSeqItem* item, const std::string& filename, std::ostream& os) { if(item->seq()->loadPlainFormat(filename)){ return true; } else { os << item->seq()->ioErrorMessage(); return false; } } static bool saveAsPlainSeqFormat(MultiValueSeqItem* item, const std::string& filename, std::ostream& os) { if(item->seq()->saveAsPlainFormat(filename)){ return true; } else { os << item->seq()->ioErrorMessage(); return false; } } template<> void MultiSeqItem::initialize(ExtensionManager* ext) { ext->itemManager().registerClass(N_("MultiValueSeqItem")); ext->itemManager().addCreationPanel( new MultiSeqItemCreationPanel(_("Number of values in a frame"))); ext->itemManager().addLoaderAndSaver( _("Plain format of a multi value sequence"), "PLAIN-MULTI-VALUE-SEQ", "*", bind(loadPlainSeqFormat, _1, _2, _3), bind(saveAsPlainSeqFormat, _1, _2, _3), ItemManager::PRIORITY_CONVERSION); } #ifdef WIN32 template class MultiSeqItem; #endif choreonoid-1.1.0+dfsg/src/Base/MultiValueSeqItem.h000066400000000000000000000006751207742442300217670ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_MULTI_VALUE_SEQ_ITEM_H_INCLUDED #define CNOID_GUIBASE_MULTI_VALUE_SEQ_ITEM_H_INCLUDED #include "MultiSeqItem.h" #include namespace cnoid { typedef MultiSeqItem MultiValueSeqItem; typedef MultiValueSeqItem::Ptr MultiValueSeqItemPtr; template<> void MultiSeqItem::initialize(ExtensionManager* ext); } #endif choreonoid-1.1.0+dfsg/src/Base/OptionManager.cpp000066400000000000000000000050321207742442300214760ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "OptionManager.h" #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { struct OptionInfo { OptionInfo() : options("Options") { } program_options::options_description options; program_options::positional_options_description positionalOptions; program_options::variables_map variables; }; OptionInfo* info = 0; } bool OptionManager::parseCommandLine(int argc, char *argv[]) { if(!info){ info = new OptionInfo; } info->options.add_options()("help,h", "show help message"); bool is_error = false; try{ program_options::store( program_options::command_line_parser(argc, argv). options(info->options).positional(info->positionalOptions).run(), info->variables); program_options::notify(info->variables); } catch (std::exception& ex) { std::cerr << "Command line option error! : " << ex.what() << std::endl; is_error = true; } bool terminated; if(info->variables.count("help") || is_error){ cout << info->options << endl; terminated = true; } else { sigOptionsParsed_(info->variables); terminated = false; } // The destructors of the OptionInfo members should be executed here // because their elements may be driven by the code instantiated in the plug-in dlls. // If the destructors are called when the program is finished after the plug-ins are // unloaded, the destructors may cause the segmentation fault // by calling the non-existent codes. delete info; info = 0; sigOptionsParsed_.disconnect_all_slots(); return !terminated; } OptionManager::OptionManager() { info = new OptionInfo; } OptionManager::~OptionManager() { } /* boost::program_options::options_description_easy_init OptionManager::addOptions() { return options.add_options(); } */ void OptionManager::addOption(const char* name, const char* description) { info->options.add_options()(name, description); } void OptionManager::addOption(const char* name, const program_options::value_semantic* s) { info->options.add_options()(name, s); } void OptionManager::addOption(const char* name, const program_options::value_semantic* s, const char* description) { info->options.add_options()(name, s, description); } void OptionManager::addPositionalOption(const char* name, int maxCount) { info->positionalOptions.add(name, maxCount); } choreonoid-1.1.0+dfsg/src/Base/OptionManager.h000066400000000000000000000026611207742442300211500ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_OPTION_MANAGER_H_INCLUDED #define CNOID_GUIBASE_OPTION_MANAGER_H_INCLUDED #include "ExtensionManager.h" #include "SignalProxy.h" #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT OptionManager { public: //boost::program_options::options_description_easy_init addOptions(); void addOption(const char* name, const char* description); void addOption(const char* name, const boost::program_options::value_semantic* s); void addOption(const char* name, const boost::program_options::value_semantic* s, const char* description); void addPositionalOption(const char* name, int maxCount); typedef boost::signal SigOptionsParsed; /** @if jp コマンドラインオプションãŒãƒ‘ースã•ã‚Œã€ã‚ªãƒ—ション解æžå‡¦ç†ã®æº–å‚™ãŒæ•´ã£ãŸã¨ã㫠発行ã•ã‚Œã‚‹ã‚·ã‚°ãƒŠãƒ«ã‚’ãƒãƒ³ãƒ‰ãƒ©é–¢æ•°ã¨æŽ¥ç¶šã™ã‚‹ã€‚ @endif */ inline SignalProxy sigOptionsParsed() { return sigOptionsParsed_; } private: OptionManager(); ~OptionManager(); bool parseCommandLine(int argc, char *argv[]); SigOptionsParsed sigOptionsParsed_; friend class ExtensionManager; friend class AppImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/OsgNormalVisualizer.cpp000066400000000000000000000161551207742442300227220ustar00rootroot00000000000000 #include "OsgNormalVisualizer.h" #include #include using namespace cnoid; namespace { class NormalExtractor : public osg::NodeVisitor { public: NormalExtractor(float normalScale = 1.0, NormalVisualizer::Mode mode = NormalVisualizer::SURFACE); void apply(osg::Geode& geode); osg::Vec3Array* getNormalLines() { return normalLines.get(); } private: NormalVisualizer::Mode mode; float scale; osg::ref_ptr normalLines; osg::Vec3Array* vertices; osg::Vec3Array::iterator vertexIter; osg::IntArray::iterator vertexIndexIter; osg::Vec3Array* normals; osg::Vec3Array::iterator normalIter; osg::UIntArray::iterator normalIndexIter; void extractOverallNormal(); void extractPrimitiveSetNormal(const osg::PrimitiveSet* primitiveSet); void extractPrimitiveNormals( const osg::PrimitiveSet* primitiveSet, osg::Geometry::AttributeBinding binding, int numVertices); void extractPolygonNormals(const osg::PrimitiveSet* primitiveSet); }; } NormalVisualizer::NormalVisualizer(Node *node, float scale, Mode mode) { NormalExtractor extractor(scale, mode); node->accept(extractor); osg::ref_ptr geom = new osg::Geometry; osg::ref_ptr lines = extractor.getNormalLines(); geom->setVertexArray(lines.get()); geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, lines->size())); osg::ref_ptr colors = new osg::Vec4Array; geom->setColorArray(colors.get()); geom->setColorBinding(osg::Geometry::BIND_OVERALL); if(mode == SURFACE){ colors->push_back(osg::Vec4(1.0, 0.0, 0.0, 1.0)); } else if(mode == VERTEX) { colors->push_back(osg::Vec4(0.0, 1.0, 0.0, 1.0)); } osg::StateSet* state = geom->getOrCreateStateSet(); state->setMode(GL_LIGHTING, osg::StateAttribute::OFF); addDrawable(geom.get()); } NormalExtractor::NormalExtractor(float normalScale, NormalVisualizer::Mode mode) : NodeVisitor(NodeVisitor::TRAVERSE_ALL_CHILDREN), mode(mode), scale(normalScale) { normalLines = new osg::Vec3Array; } void NormalExtractor::apply(osg::Geode& geode) { for(size_t i = 0; i < geode.getNumDrawables(); i++ ){ osg::Geometry* geom = dynamic_cast(geode.getDrawable(i)); if(geom){ vertices = dynamic_cast(geom->getVertexArray()); if(!vertices) continue; normals = dynamic_cast(geom->getNormalArray()); if(!normals) continue; osg::Geometry::AttributeBinding binding = geom->getNormalBinding(); if(binding == osg::Geometry::BIND_OFF) continue; if(binding == osg::Geometry::BIND_OVERALL){ extractOverallNormal(); } else { osg::Geometry::PrimitiveSetList& primitiveSets = geom->getPrimitiveSetList(); vertexIter = vertices->begin(); normalIter = normals->begin(); osg::IntArray* vertexIndices = dynamic_cast(geom->getVertexIndices()); if(vertexIndices){ vertexIndexIter = vertexIndices->begin(); } osg::UIntArray* normalIndices = dynamic_cast(geom->getNormalIndices()); if(normalIndices){ normalIndexIter = normalIndices->begin(); } osg::Geometry::PrimitiveSetList::iterator it; for(it = primitiveSets.begin(); it != primitiveSets.end(); ++it){ osg::PrimitiveSet* primitiveSet = it->get(); if(binding == osg::Geometry::BIND_PER_PRIMITIVE_SET){ extractPrimitiveSetNormal(primitiveSet); } else { switch(primitiveSet->getMode()){ case osg::PrimitiveSet::TRIANGLES: extractPrimitiveNormals(primitiveSet, binding, 3); break; case osg::PrimitiveSet::QUADS: extractPrimitiveNormals(primitiveSet, binding, 4); break; case osg::PrimitiveSet::POLYGON: extractPolygonNormals(primitiveSet); break; default: break; } } } } } } traverse(geode); } void NormalExtractor::extractOverallNormal() { osg::Vec3 v(0.0, 0.0, 0.0); const int n = vertices->size(); for(int i=0; i < n; ++i){ v += (*vertices)[i]; } v /= n; normalLines->push_back(v); normalLines->push_back(v + normals->front() * scale); } void NormalExtractor::extractPrimitiveSetNormal(const osg::PrimitiveSet* primitiveSet) { osg::Vec3 v(0.0, 0.0, 0.0); int n = primitiveSet->getNumIndices(); for(int i = 0; i < n; ++i){ v += (*vertices)[i]; } v /= n; normalLines->push_back(v); normalLines->push_back(v + *normalIter++ * scale); } void NormalExtractor::extractPrimitiveNormals (const osg::PrimitiveSet* primitiveSet, osg::Geometry::AttributeBinding binding, int numVertices) { for(size_t i=0; i < primitiveSet->getNumPrimitives(); ++i){ if(mode == NormalVisualizer::SURFACE || binding == osg::Geometry::BIND_PER_PRIMITIVE){ osg::Vec3 n(0.0, 0.0, 0.0); if(binding == osg::Geometry::BIND_PER_PRIMITIVE){ n = *(normalIter++); } else if(binding == osg::Geometry::BIND_PER_VERTEX){ for(int j = 0; j < numVertices; ++j){ n += *(normalIter++); } n /= numVertices; } osg::Vec3 v(0.0, 0.0, 0.0); for(int j = 0; j < numVertices; ++j){ v += *(vertexIter++); } v /= numVertices; normalLines->push_back(v); normalLines->push_back(v + n * scale); } else if(mode == NormalVisualizer::VERTEX){ for(int j = 0; j < numVertices; ++j){ osg::Vec3& v = *(vertexIter++); osg::Vec3& n = *(normalIter++); normalLines->push_back(v); normalLines->push_back(v + n * scale); } } } } void NormalExtractor::extractPolygonNormals(const osg::PrimitiveSet* primitiveSet) { const osg::DrawArrayLengths* polygonSizes = static_cast(primitiveSet); for(size_t i = 0; i < polygonSizes->size(); ++i){ int numVertices = (*polygonSizes)[i]; for(int j=0; j < numVertices; ++j){ osg::Vec3& v = (*vertices)[*vertexIndexIter++]; osg::Vec3& n = (*normals)[*normalIndexIter++]; normalLines->push_back(v); normalLines->push_back(v + n * scale); } } } choreonoid-1.1.0+dfsg/src/Base/OsgNormalVisualizer.h000066400000000000000000000017641207742442300223670ustar00rootroot00000000000000 #ifndef CNOID_GUIBASE_OSG_NORMAL_VISUALIZER_H_INCLUDED #define CNOID_GUIBASE_OSG_NORMAL_VISUALIZER_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT NormalVisualizer : public osg::Geode { public: enum Mode { SURFACE, VERTEX }; NormalVisualizer(osg::Node *node, float scale = 0.01, Mode mode = SURFACE); protected: ~NormalVisualizer() {} }; class CNOID_EXPORT SurfaceNormalVisualizer : public NormalVisualizer { public: SurfaceNormalVisualizer(Node *node, float scale = 0.01) : NormalVisualizer(node, scale, SURFACE) { } protected: ~SurfaceNormalVisualizer() { } }; class CNOID_EXPORT VertexNormalVisualizer : public NormalVisualizer { public: VertexNormalVisualizer( Node *node, float scale = 0.01) : NormalVisualizer(node, scale, VERTEX) { } protected: ~VertexNormalVisualizer() { } }; } #endif choreonoid-1.1.0+dfsg/src/Base/OsgOutlineFx.cpp000066400000000000000000000125211207742442300213220ustar00rootroot00000000000000 #include "OsgOutlineFx.h" #include #include #include #include #include #include #include #include using namespace cnoid; namespace osgFX { Registry::Proxy proxy(new OsgOutlineFx); } namespace { class OutlineTechnique : public osgFX::Technique { public: OutlineTechnique(const OsgOutlineFx* outline) : Technique(), outline(outline){ } bool validate(osg::State&) const { return true; } void updateColor() { if(material.valid()){ const osg::Vec4& color = outline->getColor(); material->setAmbient(osg::Material::FRONT_AND_BACK, color); material->setDiffuse(osg::Material::FRONT_AND_BACK, color); material->setEmission(osg::Material::FRONT_AND_BACK, color); } } protected: void define_passes() { static const unsigned int Override_On = osg::StateAttribute::ON | osg::StateAttribute::OVERRIDE; static const unsigned int Override_Off = osg::StateAttribute::OFF | osg::StateAttribute::OVERRIDE; { osg::StateSet* state = new osg::StateSet; osg::Stencil* stencil = new osg::Stencil; stencil->setFunction(osg::Stencil::ALWAYS, 1, ~0); //stencil->setOperation(osg::Stencil::KEEP, osg::Stencil::KEEP, osg::Stencil::REPLACE); stencil->setOperation(osg::Stencil::KEEP, osg::Stencil::REPLACE, osg::Stencil::REPLACE); state->setAttributeAndModes(stencil, Override_On); addPass(state); } { osg::StateSet* state = new osg::StateSet; osg::Stencil* stencil = new osg::Stencil; stencil->setFunction(osg::Stencil::NOTEQUAL, 1, ~0); stencil->setOperation(osg::Stencil::KEEP, osg::Stencil::KEEP, osg::Stencil::REPLACE); state->setAttributeAndModes(stencil, Override_On); /* osg::CullFace* cf = new osg::CullFace; cf->setMode(osg::CullFace::FRONT); state->setAttributeAndModes(cf, Override_On); */ osg::PolygonMode* pm = new osg::PolygonMode; //pm->setMode(osg::PolygonMode::BACK, osg::PolygonMode::LINE); //pm->setMode(osg::PolygonMode::FRONT, osg::PolygonMode::LINE); pm->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); state->setAttributeAndModes(pm, Override_On); osg::LineWidth* lw = new osg::LineWidth; lw->setWidth(outline->getWidth()); state->setAttributeAndModes(lw, Override_On); //state->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON); material = new osg::Material; material->setColorMode(osg::Material::AMBIENT_AND_DIFFUSE); updateColor(); state->setAttributeAndModes(material.get(), Override_On); state->setMode(GL_BLEND, Override_Off); //state->setMode(GL_BLEND, Override_On); state->setMode(GL_DEPTH_TEST, Override_Off); state->setTextureMode(0, GL_TEXTURE_1D, Override_Off); state->setTextureMode(0, GL_TEXTURE_2D, Override_Off); state->setTextureMode(0, GL_TEXTURE_3D, Override_Off); addPass(state); } } private: osg::ref_ptr outline; osg::ref_ptr material; }; class EnableStencilCallback : public osg::NodeCallback { public: EnableStencilCallback() { } virtual void operator()(osg::Node* node, osg::NodeVisitor* nv) { osgUtil::CullVisitor* cv = dynamic_cast(nv); if (cv) { osgUtil::RenderStage* render = cv->getRenderStage(); unsigned int mask = render->getClearMask(); if ((mask & GL_STENCIL_BUFFER_BIT) == 0) { render->setClearMask(mask | GL_STENCIL_BUFFER_BIT); render->setClearStencil(0); } } traverse(node, nv); } }; } OsgOutlineFx::OsgOutlineFx() : Effect() { width = 3.0f; color.set(1.0f, 1.0f, 1.0f, 1.0f); //addCullCallback(new EnableStencilCallback); setCullCallback(new EnableStencilCallback); } OsgOutlineFx::OsgOutlineFx(double width, const osg::Vec4& color) : Effect(), width(width), color(color) { //addCullCallback(new EnableStencilCallback); setCullCallback(new EnableStencilCallback); } OsgOutlineFx::OsgOutlineFx(const OsgOutlineFx& org, const osg::CopyOp& op) : Effect(org, op) { width = org.width; color = org.color; } OsgOutlineFx::~OsgOutlineFx() { } void OsgOutlineFx::setColor(const osg::Vec4& color) { this->color = color; if(getNumTechniques() > 0){ OutlineTechnique* tech = dynamic_cast(getTechnique(0)); if(tech){ tech->updateColor(); } } } bool OsgOutlineFx::define_techniques() { addTechnique(new OutlineTechnique(this)); return true; } choreonoid-1.1.0+dfsg/src/Base/OsgOutlineFx.h000066400000000000000000000016621207742442300207730ustar00rootroot00000000000000 #ifndef CNOID_GUIBASE_OSG_OUTLINE_FX_H_INCLUDED #define CNOID_GUIBASE_OSG_OUTLINE_FX_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT OsgOutlineFx : public osgFX::Effect { public: OsgOutlineFx(); OsgOutlineFx(double width, const osg::Vec4& color = osg::Vec4(1.0, 0.0, 0.0, 1.0)); OsgOutlineFx(const OsgOutlineFx& copy, const osg::CopyOp& op = osg::CopyOp::SHALLOW_COPY); META_Effect(cnoid, OsgOutlineFx, "OsgOutlineFx", "Stencil buffer based object outlining.", ""); inline float getWidth() const { return width; } inline const osg::Vec4& getColor() const { return color; } void setColor(const osg::Vec4& color); protected: virtual ~OsgOutlineFx(); bool define_techniques(); private: float width; osg::Vec4 color; }; }; #endif choreonoid-1.1.0+dfsg/src/Base/OsgViewer.cpp000066400000000000000000000251151207742442300206510ustar00rootroot00000000000000 #include "OsgViewer.h" #include #include using namespace std; using namespace osgGA; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } OsgWidget::OsgWidget(QWidget* parent, const QGLWidget* shareWidget, Qt::WindowFlags f) : QGLWidget(parent, shareWidget, f) { graphicsWindow_ = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height()); setFocusPolicy(Qt::WheelFocus); } OsgWidget::~OsgWidget() { if(TRACE_FUNCTIONS){ cout << "OsgWidget::~OsgWidget()" << endl; } } void OsgWidget::resizeGL(int width, int height) { if(width > 0 && height > 0){ graphicsWindow_->getEventQueue()->windowResize(0, 0, width, height); graphicsWindow_->resized(0, 0, width, height); } } /** returns a osgGA::GUIEventAdapter::MouseButtonMask value */ int OsgWidget::convertToOsgMouseButtonMask(int qtMouseButtons) { int mask = 0; if(qtMouseButtons & Qt::LeftButton){ mask |= GUIEventAdapter::LEFT_MOUSE_BUTTON; } if(qtMouseButtons & Qt::MidButton){ mask |= GUIEventAdapter::MIDDLE_MOUSE_BUTTON; } if(qtMouseButtons & Qt::RightButton){ mask |= GUIEventAdapter::RIGHT_MOUSE_BUTTON; } return mask; } /** returns a osgGA::GUIEventAdapter::KeySymbol value */ int OsgWidget::convertToOsgKeySymbol(int qtKeySymbol) { switch(qtKeySymbol){ case Qt::Key_Escape: return GUIEventAdapter::KEY_Escape; case Qt::Key_Tab: return GUIEventAdapter::KEY_Tab; case Qt::Key_Backspace: return GUIEventAdapter::KEY_BackSpace; case Qt::Key_Return: return GUIEventAdapter::KEY_Return; case Qt::Key_Enter: return GUIEventAdapter::KEY_Return; case Qt::Key_Insert: return GUIEventAdapter::KEY_Insert; case Qt::Key_Delete: return GUIEventAdapter::KEY_Delete; case Qt::Key_Pause: return GUIEventAdapter::KEY_Pause; case Qt::Key_Print: return GUIEventAdapter::KEY_Print; case Qt::Key_SysReq: return GUIEventAdapter::KEY_Sys_Req; case Qt::Key_Clear: return GUIEventAdapter::KEY_Clear; case Qt::Key_Home: return GUIEventAdapter::KEY_Home; case Qt::Key_End: return GUIEventAdapter::KEY_End; case Qt::Key_Left: return GUIEventAdapter::KEY_Left; case Qt::Key_Up: return GUIEventAdapter::KEY_Up; case Qt::Key_Right: return GUIEventAdapter::KEY_Right; case Qt::Key_Down: return GUIEventAdapter::KEY_Down; case Qt::Key_PageUp: return GUIEventAdapter::KEY_Page_Up; case Qt::Key_PageDown: return GUIEventAdapter::KEY_Page_Down; case Qt::Key_Shift: return GUIEventAdapter::KEY_Shift_L; case Qt::Key_Control: return GUIEventAdapter::KEY_Control_L; case Qt::Key_Meta: return GUIEventAdapter::KEY_Meta_L; case Qt::Key_Alt: return GUIEventAdapter::KEY_Alt_L; case Qt::Key_CapsLock: return GUIEventAdapter::KEY_Caps_Lock; case Qt::Key_NumLock: return GUIEventAdapter::KEY_Num_Lock; case Qt::Key_ScrollLock: return GUIEventAdapter::KEY_Scroll_Lock; case Qt::Key_F1: return GUIEventAdapter::KEY_F1; case Qt::Key_F2: return GUIEventAdapter::KEY_F2; case Qt::Key_F3: return GUIEventAdapter::KEY_F3; case Qt::Key_F4: return GUIEventAdapter::KEY_F4; case Qt::Key_F5: return GUIEventAdapter::KEY_F5; case Qt::Key_F6: return GUIEventAdapter::KEY_F6; case Qt::Key_F7: return GUIEventAdapter::KEY_F7; case Qt::Key_F8: return GUIEventAdapter::KEY_F8; case Qt::Key_F9: return GUIEventAdapter::KEY_F9; case Qt::Key_F10: return GUIEventAdapter::KEY_F10; case Qt::Key_F11: return GUIEventAdapter::KEY_F11; case Qt::Key_F12: return GUIEventAdapter::KEY_F12; case Qt::Key_F13: return GUIEventAdapter::KEY_F13; case Qt::Key_F14: return GUIEventAdapter::KEY_F14; case Qt::Key_F15: return GUIEventAdapter::KEY_F15; case Qt::Key_F16: return GUIEventAdapter::KEY_F16; case Qt::Key_F17: return GUIEventAdapter::KEY_F17; case Qt::Key_F18: return GUIEventAdapter::KEY_F18; case Qt::Key_F19: return GUIEventAdapter::KEY_F19; case Qt::Key_F20: return GUIEventAdapter::KEY_F20; case Qt::Key_F21: return GUIEventAdapter::KEY_F21; case Qt::Key_F22: return GUIEventAdapter::KEY_F22; case Qt::Key_F23: return GUIEventAdapter::KEY_F23; case Qt::Key_F24: return GUIEventAdapter::KEY_F24; case Qt::Key_F25: return GUIEventAdapter::KEY_F25; case Qt::Key_F26: return GUIEventAdapter::KEY_F26; case Qt::Key_F27: return GUIEventAdapter::KEY_F27; case Qt::Key_F28: return GUIEventAdapter::KEY_F28; case Qt::Key_F29: return GUIEventAdapter::KEY_F29; case Qt::Key_F30: return GUIEventAdapter::KEY_F30; case Qt::Key_F31: return GUIEventAdapter::KEY_F31; case Qt::Key_F32: return GUIEventAdapter::KEY_F32; case Qt::Key_F33: return GUIEventAdapter::KEY_F33; case Qt::Key_F34: return GUIEventAdapter::KEY_F34; case Qt::Key_F35: return GUIEventAdapter::KEY_F35; case Qt::Key_Super_L: return GUIEventAdapter::KEY_Super_L; case Qt::Key_Super_R: return GUIEventAdapter::KEY_Super_R; case Qt::Key_Menu: return GUIEventAdapter::KEY_Menu; case Qt::Key_Hyper_L: return GUIEventAdapter::KEY_Hyper_L; case Qt::Key_Hyper_R: return GUIEventAdapter::KEY_Hyper_R; case Qt::Key_Help: return GUIEventAdapter::KEY_Help; case Qt::Key_Space: return GUIEventAdapter::KEY_Space; case Qt::Key_Exclam: return 0x21; case Qt::Key_QuoteDbl: return 0x22; case Qt::Key_NumberSign: return 0x23; case Qt::Key_Dollar: return 0x24; case Qt::Key_Percent: return 0x25; case Qt::Key_Ampersand: return 0x26; case Qt::Key_Apostrophe: return 0x27; case Qt::Key_ParenLeft: return 0x28; case Qt::Key_ParenRight: return 0x29; case Qt::Key_Asterisk: return 0x2a; case Qt::Key_Plus: return 0x2b; case Qt::Key_Comma: return 0x2c; case Qt::Key_Minus: return 0x2d; case Qt::Key_Period: return 0x2e; case Qt::Key_Slash: return 0x2f; case Qt::Key_0: return 0x30; case Qt::Key_1: return 0x31; case Qt::Key_2: return 0x32; case Qt::Key_3: return 0x33; case Qt::Key_4: return 0x34; case Qt::Key_5: return 0x35; case Qt::Key_6: return 0x36; case Qt::Key_7: return 0x37; case Qt::Key_8: return 0x38; case Qt::Key_9: return 0x39; case Qt::Key_Colon: return 0x3a; case Qt::Key_Semicolon: return 0x3b; case Qt::Key_Less: return 0x3c; case Qt::Key_Equal: return 0x3d; case Qt::Key_Greater: return 0x3e; case Qt::Key_Question: return 0x3f; case Qt::Key_At: return 0x40; case Qt::Key_A: return 0x41; case Qt::Key_B: return 0x42; case Qt::Key_C: return 0x43; case Qt::Key_D: return 0x44; case Qt::Key_E: return 0x45; case Qt::Key_F: return 0x46; case Qt::Key_G: return 0x47; case Qt::Key_H: return 0x48; case Qt::Key_I: return 0x49; case Qt::Key_J: return 0x4a; case Qt::Key_K: return 0x4b; case Qt::Key_L: return 0x4c; case Qt::Key_M: return 0x4d; case Qt::Key_N: return 0x4e; case Qt::Key_O: return 0x4f; case Qt::Key_P: return 0x50; case Qt::Key_Q: return 0x51; case Qt::Key_R: return 0x52; case Qt::Key_S: return 0x53; case Qt::Key_T: return 0x54; case Qt::Key_U: return 0x55; case Qt::Key_V: return 0x56; case Qt::Key_W: return 0x57; case Qt::Key_X: return 0x58; case Qt::Key_Y: return 0x59; case Qt::Key_Z: return 0x5a; case Qt::Key_BracketLeft: return 0x5b; case Qt::Key_Backslash: return 0x5c; case Qt::Key_BracketRight: return 0x5d; case Qt::Key_AsciiCircum: return 0x5e; case Qt::Key_Underscore: return 0x5f; case Qt::Key_QuoteLeft: return 0x60; case Qt::Key_BraceLeft: return 0x7b; case Qt::Key_Bar: return 0x7c; case Qt::Key_BraceRight: return 0x7d; case Qt::Key_AsciiTilde: return 0x7e; default: break; } return 0; } /** returns a osgGA::GUIEventAdapter::ModKeyMask value */ int OsgWidget::convertToOsgModKeyMask(int qtModifiers) { int mask = 0; if(qtModifiers & Qt::ShiftModifier){ mask |= GUIEventAdapter::MODKEY_SHIFT; } if(qtModifiers & Qt::ControlModifier){ mask |= GUIEventAdapter::MODKEY_CTRL; } if(qtModifiers & Qt::AltModifier){ mask |= GUIEventAdapter::MODKEY_ALT; } if(qtModifiers & Qt::MetaModifier){ mask |= GUIEventAdapter::MODKEY_META; } return mask; } /** returns a osgGA::GUIEventAdapter::ScrollingMotion value */ int OsgWidget::convertToOsgScrollingMotion(QWheelEvent* event) { int delta = event->delta(); switch(event->orientation()){ case Qt::Horizontal: if(delta > 0){ return GUIEventAdapter::SCROLL_RIGHT; } else if(delta < 0){ return GUIEventAdapter::SCROLL_LEFT; } break; case Qt::Vertical: if(delta > 0){ return GUIEventAdapter::SCROLL_UP; } else if(delta < 0){ return GUIEventAdapter::SCROLL_DOWN; } break; } return GUIEventAdapter::SCROLL_NONE; } OsgViewer::OsgViewer(QWidget* parent, const QGLWidget* shareWidget, Qt::WindowFlags f) : OsgWidget(parent, shareWidget, f) { getCamera()->setViewport(new osg::Viewport(0, 0, width(), height())); getCamera()->setProjectionMatrixAsPerspective( 30.0f, static_cast(width()) / static_cast(height()), 1.0f, 10000.0f); getCamera()->setGraphicsContext(graphicsWindow()); setThreadingModel(osgViewer::Viewer::SingleThreaded); isCollisionVisibleMode_ = false; } OsgViewer::~OsgViewer() { if(TRACE_FUNCTIONS){ cout << "OsgViewer::~OsgViewer()" << endl; } } void OsgViewer::requestRedraw() { updateGL(); } void OsgViewer::paintGL() { frame(); //updateTraversal(); //renderingTraversals(); } choreonoid-1.1.0+dfsg/src/Base/OsgViewer.h000066400000000000000000000030051207742442300203100ustar00rootroot00000000000000 #ifndef CNOID_GUIBASE_OSG_VIEWER_H_INCLUDED #define CNOID_GUIBASE_OSG_VIEWER_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT OsgWidget : public QGLWidget { osg::ref_ptr graphicsWindow_; public: OsgWidget(QWidget* parent = 0, const QGLWidget* shareWidget = 0, Qt::WindowFlags f = 0); virtual ~OsgWidget(); inline osgViewer::GraphicsWindow* graphicsWindow() { return graphicsWindow_.get(); } inline const osgViewer::GraphicsWindow* graphicsWindow() const { return graphicsWindow_.get(); } static int convertToOsgMouseButtonMask(int qtButton); static int convertToOsgKeySymbol(int qtKeySymbol); static int convertToOsgModKeyMask(int qtModifiers); static int convertToOsgScrollingMotion(QWheelEvent* event); protected: virtual void resizeGL(int width, int height); }; class CNOID_EXPORT OsgViewer : public osgViewer::Viewer, public OsgWidget { public: OsgViewer(QWidget* parent = 0, const QGLWidget* shareWidget = 0, Qt::WindowFlags f = 0); virtual ~OsgViewer(); virtual void requestRedraw(); virtual void paintGL(); /// temorary hack inline bool isCollisionVisibleMode() { return isCollisionVisibleMode_; } private: bool isCollisionVisibleMode_; friend class SceneViewImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/Plugin.cpp000066400000000000000000000032431207742442300201730ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "Plugin.h" #include "Item.h" #include "ToolBar.h" #include "View.h" #include "Licenses.h" #include using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class PluginImpl { public: PluginImpl(const char* name); string name; vector requisites; vector oldNames; }; } Plugin::Plugin(const char* name) : ExtensionManager(name, true) { impl = new PluginImpl(name); } PluginImpl::PluginImpl(const char* name) : name(name) { } Plugin::~Plugin() { delete impl; } const char* Plugin::name() { return impl->name.c_str(); } bool Plugin::initialize() { return true; } bool Plugin::finalize() { return true; } /** When the plugin depends on some other plugins, please specify the plugins to depend with this function in the constructor. */ void Plugin::require(const char* pluginName) { impl->requisites.push_back(pluginName); } /** Obsolete. Please use require() instead of this function. */ void Plugin::depend(const char* pluginName) { require(pluginName); } const char* Plugin::requisite(int index) { return impl->requisites[index].c_str(); } int Plugin::numRequisites() { return impl->requisites.size(); } void Plugin::addOldName(const char* name) { impl->oldNames.push_back(name); } const char* Plugin::oldName(int index) { return impl->oldNames[index].c_str(); } int Plugin::numOldNames() { return impl->oldNames.size(); } const char* Plugin::description() { return ""; } const char* Plugin::LGPLtext() { return cnoid::LGPLtext(); } choreonoid-1.1.0+dfsg/src/Base/Plugin.h000066400000000000000000000035111207742442300176360ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BASE_PLUGIN_H_INCLUDED #define CNOID_BASE_PLUGIN_H_INCLUDED #include "ExtensionManager.h" #include "exportdecl.h" namespace cnoid { class Item; class View; class ToolBar; class PluginImpl; class CNOID_EXPORT Plugin : public ExtensionManager { public: typedef Plugin* (*PluginEntry)(); Plugin(const char* name); virtual ~Plugin(); const char* name(); virtual bool initialize(); virtual bool finalize(); const char* requisite(int index); int numRequisites(); const char* oldName(int index); int numOldNames(); virtual const char* description(); protected: void setPluginScope(Item* item); void setPluginScope(View* view); void setPluginScope(ToolBar* toolBar); void require(const char* pluginName); void depend(const char* pluginName); /** When the plugin name is changed but the old project files should be loadable, specify old names of the plugin with this function in the constructor. */ void addOldName(const char* name); static const char* LGPLtext(); private: Plugin(const Plugin& org); // disable the copy constructor PluginImpl* impl; }; } #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) #define CNOID_IMPLEMENT_PLUGIN_ENTRY(PluginTypeName) \ extern "C" __declspec(dllexport) cnoid::Plugin* getChoreonoidPlugin() \ { \ return new PluginTypeName(); \ } #else #define CNOID_IMPLEMENT_PLUGIN_ENTRY(PluginTypeName) \ extern "C" cnoid::Plugin* getChoreonoidPlugin() \ { \ return new PluginTypeName(); \ } #endif #endif choreonoid-1.1.0+dfsg/src/Base/PluginManager.cpp000066400000000000000000000371531207742442300214750ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "PluginManager.h" #include "Plugin.h" #include "ExtensionManager.h" #include "MenuManager.h" #include "MessageView.h" #include "DescriptionDialog.h" #include "App.h" #include "LazyCaller.h" #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; #ifdef Q_OS_WIN32 static const char* DLL_PREFIX = ""; static const char* DLL_SUFFIX = "dll"; static const char* PATH_DELIMITER = ";"; # ifdef CNOID_DEBUG static const char* DEBUG_SUFFIX = "d"; # else static const char* DEBUG_SUFFIX = ""; # endif #else # ifdef Q_OS_MAC static const char* DLL_PREFIX = ""; static const char* DLL_SUFFIX = "dylib"; static const char* PATH_DELIMITER = ":"; static const char* DEBUG_SUFFIX = ""; # else static const char* DLL_PREFIX = "lib"; static const char* DLL_SUFFIX = "so"; static const char* PATH_DELIMITER = ":"; static const char* DEBUG_SUFFIX = ""; # endif #endif namespace cnoid { class PluginManagerImpl { public: PluginManagerImpl(); ~PluginManagerImpl(); MessageView* mv; QRegExp pluginNamePattern; struct PluginInfo; typedef shared_ptr PluginInfoPtr; typedef map PluginMap; struct PluginInfo { PluginInfo(){ plugin = 0; status = PluginManager::NOT_LOADED; aboutMenuItem = 0; aboutDialog = 0; } QLibrary dll; std::string pathString; Plugin* plugin; string name; vector requisites; vector dependents; int status; QAction* aboutMenuItem; DescriptionDialog* aboutDialog; }; vector allPluginInfos; PluginMap nameToActivePluginInfoMap; PluginMap pathToPluginInfoMap; typedef multimap MultiNameMap; MultiNameMap oldNameToCurrentPluginNameMap; vector pluginsToUnload; LazyCaller unloadingRequest; void clearUnusedPlugins(); void scanPluginFilesInDefaultPath(const std::string& pathList); void scanPluginFilesInDirectoyOfExecFile(); void scanPluginFiles(const std::string& pathString, bool isRecursive); void loadAllPlugins(); bool unloadAllPlugins(); bool loadPlugin(int index); void onAboutDialogTriggered(PluginInfo* info); const char* guessActualPluginName(const std::string& name); bool unloadPlugin(int index); bool unloadPlugin(PluginInfoPtr info); void onUnloadingRequest(); }; } PluginManager* PluginManager::instance() { static PluginManager* pluginManager = new PluginManager(); return pluginManager; } PluginManager::PluginManager() { impl = new PluginManagerImpl(); } PluginManagerImpl::PluginManagerImpl() : mv(MessageView::mainInstance()) { pluginNamePattern.setPattern(QString(DLL_PREFIX) + "Cnoid.+Plugin" + DEBUG_SUFFIX + "\\." + DLL_SUFFIX); // for the base module PluginInfoPtr info(new PluginInfo); info->name = "Base"; nameToActivePluginInfoMap.insert(make_pair(string("Base"), info)); unloadingRequest.set(bind(&PluginManagerImpl::onUnloadingRequest, this), IDLE_PRIORITY_LOW); } PluginManager::~PluginManager() { delete impl; } PluginManagerImpl::~PluginManagerImpl() { unloadAllPlugins(); } int PluginManager::numPlugins() { return impl->allPluginInfos.size(); } const std::string& PluginManager::pluginPath(int index) { return impl->allPluginInfos[index]->pathString; } const std::string& PluginManager::pluginName(int index) { return impl->allPluginInfos[index]->name; } int PluginManager::pluginStatus(int index) { return impl->allPluginInfos[index]->status; } /** This function scans plugin files in the path list. @param pathList List of the directories to scan, which is specified with the same format as the PATH environment varibale. */ void PluginManager::scanPluginFilesInPathList(const std::string& pathList) { impl->scanPluginFilesInDefaultPath(pathList); } void PluginManagerImpl::scanPluginFilesInDefaultPath(const std::string& pathList) { char_separator sep(PATH_DELIMITER); tokenizer< char_separator > paths(pathList, sep); tokenizer< char_separator >::iterator p; for(p = paths.begin(); p != paths.end(); ++p){ const string& path = *p; scanPluginFiles(path, false); } } void PluginManager::scanPluginFilesInDirectoyOfExecFile() { impl->scanPluginFilesInDirectoyOfExecFile(); } void PluginManagerImpl::scanPluginFilesInDirectoyOfExecFile() { string pluginDirectory( str(boost::format("%1%/%2%") % App::topDirectory() % CNOID_PLUGIN_SUBDIR)); scanPluginFiles(pluginDirectory, false); } void PluginManager::scanPluginFiles(const std::string& pathString) { impl->scanPluginFiles(pathString, false); } void PluginManagerImpl::scanPluginFiles(const std::string& pathString, bool isRecursive) { filesystem::path pluginPath(pathString); if(filesystem::exists(pluginPath)){ if(filesystem::is_directory(pluginPath)){ if(!isRecursive){ filesystem::directory_iterator end; for(filesystem::directory_iterator it(pluginPath); it != end; ++it){ const filesystem::path& filepath = *it; scanPluginFiles(filepath.file_string(), true); } } } else { if(pluginNamePattern.exactMatch(pluginPath.leaf().c_str())){ PluginMap::iterator p = pathToPluginInfoMap.find(pathString); if(p == pathToPluginInfoMap.end()){ PluginInfoPtr info(new PluginInfo); info->pathString = pathString; allPluginInfos.push_back(info); pathToPluginInfoMap[pathString] = info; } } } } } void PluginManager::clearUnusedPlugins() { impl->clearUnusedPlugins(); } void PluginManagerImpl::clearUnusedPlugins() { vector oldList = allPluginInfos; allPluginInfos.clear(); for(size_t i=0; i < oldList.size(); ++i){ PluginInfoPtr info = oldList[i]; if(info->status == PluginManager::ACTIVE){ allPluginInfos.push_back(info); } else { pathToPluginInfoMap.erase(info->pathString); } } } void PluginManager::loadAllPlugins() { impl->loadAllPlugins(); } void PluginManagerImpl::loadAllPlugins() { size_t totalNumActivated = 0; while(true){ size_t numActivated = 0; for(size_t i=0; i < allPluginInfos.size(); ++i){ int status = allPluginInfos[i]->status; if(status == PluginManager::NOT_LOADED || status == PluginManager::LOADED){ if(loadPlugin(i)){ numActivated++; totalNumActivated++; } } } if(numActivated == 0 || totalNumActivated == allPluginInfos.size()){ break; } } } bool PluginManager::loadPlugin(int index) { return impl->loadPlugin(index); } bool PluginManagerImpl::loadPlugin(int index) { QString errorMessage; PluginInfoPtr info = allPluginInfos[index]; if(info->status == PluginManager::ACTIVE){ mv->putln(format(_("Plugin file \"%1%\" has already been activated.")) % info->pathString); } else if(info->status == PluginManager::NOT_LOADED){ mv->putln(format(_("Loading plugin file \"%1%\"")) % info->pathString); mv->flush(); info->dll.setFileName(info->pathString.c_str()); if(!(info->dll.load())){ errorMessage = info->dll.errorString(); } else { void* symbol = info->dll.resolve("getChoreonoidPlugin"); if(!symbol){ info->status = PluginManager::INVALID; errorMessage = _("The plugin entry function \"getChoreonoidPlugin\" is not found.\n"); errorMessage += info->dll.errorString(); } else { Plugin::PluginEntry getCnoidPluginFunc = (Plugin::PluginEntry)(symbol); info->plugin = getCnoidPluginFunc(); if(!info->plugin){ info->status = PluginManager::INVALID; errorMessage = _("The plugin object cannot be created."); } else { info->status = PluginManager::LOADED; info->name = info->plugin->name(); int numRequisites = info->plugin->numRequisites(); for(int i=0; i < numRequisites; ++i){ info->requisites.push_back(info->plugin->requisite(i)); } } } } } if(info->status == PluginManager::LOADED){ PluginMap::iterator p = nameToActivePluginInfoMap.find(info->name); if(p != nameToActivePluginInfoMap.end()){ info->status = PluginManager::CONFLICT; PluginInfoPtr loadedPlugin = p->second; errorMessage = str(format(_("The plugin conflicts with plugin \"%1%\".")) % loadedPlugin->pathString).c_str(); } else { bool requisitesActive = true; // check whether all the required plugins have already been active for(size_t i=0; i < info->requisites.size(); ++i){ PluginMap::iterator q = nameToActivePluginInfoMap.find(info->requisites[i]); if(q == nameToActivePluginInfoMap.end()){ requisitesActive = false; break; } } if(requisitesActive){ if(!info->plugin->initialize()){ info->status = PluginManager::INVALID; errorMessage = _("The plugin object cannot be intialized."); } else { info->status = PluginManager::ACTIVE; nameToActivePluginInfoMap[info->name] = info; // add this plugin to dependent list of the requisites for(size_t i=0; i < info->requisites.size(); ++i){ PluginMap::iterator p = nameToActivePluginInfoMap.find(info->requisites[i]); if(p != nameToActivePluginInfoMap.end()){ p->second->dependents.push_back(info->name); } } // set an about dialog info->plugin->menuManager().setPath("/Help").setPath(_("About Plugins")) .addItem(str(format(_("About %1% Plugin")) % info->name).c_str()) ->sigTriggered().connect( bind(&PluginManagerImpl::onAboutDialogTriggered, this, info.get())); // register old names int numOldNames = info->plugin->numOldNames(); for(int i=0; i < numOldNames; ++i){ oldNameToCurrentPluginNameMap.insert( make_pair(info->plugin->oldName(i), info->name)); } mv->putln(format(_("%1%-plugin has been activated.")) % info->name); mv->flush(); ExtensionManager::notifySystemUpdate(); } } } } if(!errorMessage.isEmpty()){ mv->putln(_("Loading the plugin failed.")); mv->putln(errorMessage); mv->flush(); } return (info->status == PluginManager::ACTIVE); } void PluginManagerImpl::onAboutDialogTriggered(PluginInfo* info) { if(!info->aboutDialog){ info->aboutDialog = new DescriptionDialog(); info->aboutDialog->setWindowTitle(str(format(_("About %1% Plugin")) % info->name).c_str()); info->aboutDialog->setDescription(info->plugin->description()); } info->aboutDialog->show(); } const char* PluginManager::guessActualPluginName(const std::string& name) { return impl->guessActualPluginName(name); } const char* PluginManagerImpl::guessActualPluginName(const std::string& name) { PluginMap::iterator p = nameToActivePluginInfoMap.find(name); if(p != nameToActivePluginInfoMap.end()){ return p->second->name.c_str(); } MultiNameMap::iterator q, upper_bound; std::pair range = oldNameToCurrentPluginNameMap.equal_range(name); for(MultiNameMap::iterator q = range.first; q != range.second; ++q){ const string& candidate = q->second; PluginMap::iterator r = nameToActivePluginInfoMap.find(candidate); if(r != nameToActivePluginInfoMap.end()){ return r->second->name.c_str(); } } return 0; } bool PluginManager::unloadPlugin(int index) { return impl->unloadPlugin(index); } bool PluginManagerImpl::unloadPlugin(int index) { return unloadPlugin(allPluginInfos[index]); } bool PluginManagerImpl::unloadPlugin(PluginInfoPtr info) { bool unloadFailed = false; if(info->status == PluginManager::ACTIVE){ if(info->plugin){ for(size_t i=0; i < info->dependents.size(); ++i){ PluginMap::iterator p = nameToActivePluginInfoMap.find(info->dependents[i]); if(p != nameToActivePluginInfoMap.end()){ PluginInfoPtr& dependentInfo = p->second; if(dependentInfo->status == PluginManager::ACTIVE || dependentInfo->status == PluginManager::LOADED){ if(!unloadPlugin(dependentInfo)){ unloadFailed = true; } } } } if(unloadFailed){ mv->putln(format(_("Plugin %1% cannot be finalized because its dependent(s) cannot be finalized.")) % info->name); mv->flush(); } else if(info->plugin->finalize()){ info->status = PluginManager::LOADED; } else { unloadFailed = true; mv->putln(format(_("Plugin %1% cannot be finalized.")) % info->name); mv->flush(); } } } if(!unloadFailed && info->status == PluginManager::LOADED){ delete info->plugin; info->plugin = 0; if(info->aboutDialog){ delete info->aboutDialog; info->aboutDialog = 0; } nameToActivePluginInfoMap.erase(info->name); info->status = PluginManager::NOT_LOADED; ExtensionManager::notifySystemUpdate(); pluginsToUnload.push_back(info); unloadingRequest.request(); } return !unloadFailed; } void PluginManagerImpl::onUnloadingRequest() { for(size_t i=0; i < pluginsToUnload.size(); ++i){ PluginInfoPtr& info = pluginsToUnload[i]; info->dll.unload(); mv->putln(format(_("Plugin dll %1% has been unloaded.")) % info->pathString); mv->flush(); } pluginsToUnload.clear(); } bool PluginManager::unloadAllPlugins() { return impl->unloadAllPlugins(); } bool PluginManagerImpl::unloadAllPlugins() { bool failed = false; for(size_t i=0; i < allPluginInfos.size(); ++i){ if(!unloadPlugin(i)){ failed = true; } } return !failed; } choreonoid-1.1.0+dfsg/src/Base/PluginManager.h000066400000000000000000000020631207742442300211320ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_PLUGIN_MANAGER_H_INCLUDED #define CNOID_GUIBASE_PLUGIN_MANAGER_H_INCLUDED #include namespace cnoid { class PluginManagerImpl; class PluginManager { public: static PluginManager* instance(); ~PluginManager(); void scanPluginFilesInPathList(const std::string& pathList); void scanPluginFilesInDirectoyOfExecFile(); void scanPluginFiles(const std::string& pathString); void clearUnusedPlugins(); void loadAllPlugins(); bool unloadAllPlugins(); int numPlugins(); const std::string& pluginPath(int index); const std::string& pluginName(int index); enum PluginStatus { NOT_LOADED, LOADED, ACTIVE, INVALID, CONFLICT }; int pluginStatus(int index); bool loadPlugin(int index); bool unloadPlugin(int index); const char* guessActualPluginName(const std::string& name); private: PluginManager(); PluginManagerImpl* impl; }; }; #endif choreonoid-1.1.0+dfsg/src/Base/ProjectManager.cpp000066400000000000000000000345201207742442300216400ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ProjectManager.h" #include "RootItem.h" #include "ItemManager.h" #include "MessageView.h" #include "ToolBar.h" #include "Archive.h" #include "ItemTreeArchiver.h" #include "ExtensionManager.h" #include "OptionManager.h" #include "MenuManager.h" #include "App.h" #include "AppConfig.h" #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { ProjectManager* instance_ = 0; } namespace cnoid { class ProjectManagerImpl { public: ProjectManagerImpl(ExtensionManager* em); ~ProjectManagerImpl(); template void restoreObjectStates(Archive* projectArchive, Archive* states, const vector& objects); void loadProject(const string& filename, bool isInvokingApplication); template bool storeObjects(ArchivePtr& parentArchive, const char* key, vector objects); void saveProject(const string& filename); void overwriteCurrentProject(); void onSigOptionsParsed(boost::program_options::variables_map& v); void openDialogToLoadProject(); void openDialogToSaveProject(); void onPerspectiveCheckToggled(); void connectArchiver( const std::string& name, boost::function storeFunction, boost::function restoreFunction); ItemTreeArchiver itemTreeArchiver; MainWindow* mainWindow; MessageView* messageView; string lastAccessedProjectFile; Action* perspectiveCheck; struct ArchiverInfo { boost::function storeFunction; boost::function restoreFunction; }; typedef map ArchiverMap; typedef map ArchiverMapMap; ArchiverMapMap archivers; }; } ProjectManager* ProjectManager::instance() { return instance_; } void ProjectManager::initialize(ExtensionManager* em) { if(!instance_){ instance_ = em->manage(new ProjectManager(em)); } } ProjectManager::ProjectManager(ExtensionManager* em) { impl = new ProjectManagerImpl(em); } ProjectManagerImpl::ProjectManagerImpl(ExtensionManager* em) { YamlMappingPtr config = AppConfig::archive()->openMapping("ProjectManager"); MenuManager& mm = em->menuManager(); mm.setPath("/File"); mm.addItem(_("Open Project")) ->sigTriggered().connect(bind(&ProjectManagerImpl::openDialogToLoadProject, this)); mm.addItem(_("Save Project")) ->sigTriggered().connect(bind(&ProjectManagerImpl::overwriteCurrentProject, this)); mm.addItem(_("Save Project As")) ->sigTriggered().connect(bind(&ProjectManagerImpl::openDialogToSaveProject, this)); perspectiveCheck = mm.setPath(N_("Project File Options")).addCheckItem(_("Perspective")); perspectiveCheck->setChecked(config->get("storePerspective", false)); perspectiveCheck->sigToggled().connect(bind(&ProjectManagerImpl::onPerspectiveCheckToggled, this)); mm.setPath("/File"); mm.addSeparator(); OptionManager& om = em->optionManager(); om.addOption("project", program_options::value< vector >(), "load a project file"); om.addPositionalOption("project", 1); om.sigOptionsParsed().connect(bind(&ProjectManagerImpl::onSigOptionsParsed, this, _1)); mainWindow = MainWindow::instance(); messageView = MessageView::mainInstance(); } ProjectManager::~ProjectManager() { delete impl; instance_ = 0; } ProjectManagerImpl::~ProjectManagerImpl() { } template void ProjectManagerImpl::restoreObjectStates (Archive* projectArchive, Archive* states, const vector& objects) { for(size_t i=0; i < objects.size(); ++i){ TObject* object = objects[i]; Archive* state = states->findSubArchive(object->objectName().toStdString()); if(state->isValid()){ state->inheritSharedInfoFrom(projectArchive); object->restoreState(*state); } } } void ProjectManager::loadProject(const std::string& filename) { impl->loadProject(filename, false); } void ProjectManagerImpl::loadProject(const std::string& filename, bool isInvokingApplication) { YamlReader reader; reader.setMappingClass(); try { messageView->putln(); messageView->notify(str(format(_("Loading project file \"%1%\" ...")) % filename)); if(!isInvokingApplication){ messageView->flush(); } bool result = false; if(!reader.load(filename)){ messageView->put(reader.errorMessage() + "\n"); } else if(reader.numDocuments() == 0){ messageView->put(_("The project file is empty.\n")); } else { Archive* archive = static_cast(reader.document()->toMapping()); archive->initSharedInfo(filename); MainWindow* mainWindow = MainWindow::instance(); if(isInvokingApplication){ if(perspectiveCheck->isChecked()){ mainWindow->setInitialLayout(archive); } mainWindow->show(); messageView->flush(); mainWindow->repaint(); } else { if(perspectiveCheck->isChecked()){ mainWindow->restoreLayout(archive); } } string currentFileDialogDirectory; if(archive->readRelocatablePath("currentFileDialogDirectory", currentFileDialogDirectory)){ AppConfig::archive()->writePath( "currentFileDialogDirectory", currentFileDialogDirectory); } Archive* items = archive->findSubArchive("items"); if(items->isValid()){ items->inheritSharedInfoFrom(archive); result = itemTreeArchiver.restore(items, RootItem::mainInstance()); } Archive* viewStates = archive->findSubArchive("views"); if(viewStates->isValid()){ restoreObjectStates(archive, viewStates, mainWindow->allViews()); } Archive* barStates = archive->findSubArchive("toolbars"); if(barStates->isValid()){ restoreObjectStates(archive, barStates, mainWindow->allToolBars()); } ArchiverMapMap::iterator p; for(p = archivers.begin(); p != archivers.end(); ++p){ const string& moduleName = p->first; Archive* moduleArchive = archive->findSubArchive(moduleName); if(moduleArchive->isValid()){ ArchiverMap::iterator q; for(q = p->second.begin(); q != p->second.end(); ++q){ const string& objectName = q->first; Archive* objArchive = moduleArchive->findSubArchive(objectName); if(objArchive->isValid()){ ArchiverInfo& info = q->second; objArchive->inheritSharedInfoFrom(archive); info.restoreFunction(*objArchive); } } } } archive->callPostProcesses(); } if(result){ messageView->notify(str(format(_("Project \"%1%\" has successfully been loaded.")) % filename)); lastAccessedProjectFile = filename; } else { messageView->notify(str(format(_("Project \"%1%\" cannot be loaded.")) % filename)); lastAccessedProjectFile.clear(); } } catch (const YamlNode::Exception& ex){ messageView->put(ex.message()); } } template bool ProjectManagerImpl::storeObjects (ArchivePtr& parentArchive, const char* key, vector objects) { bool result = true; if(!objects.empty()){ YamlMappingPtr archives = new YamlMapping(); archives->setKeyQuoteStyle(YAML_DOUBLE_QUOTED); for(size_t i=0; i < objects.size(); ++i){ TObject* object = objects[i]; string name = object->objectName().toStdString(); if(!name.empty()){ ArchivePtr archive = new Archive(); archive->inheritSharedInfoFrom(parentArchive); if(object->storeState(*archive) && !archive->empty()){ archives->insert(name, archive); } } } if(!archives->empty()){ parentArchive->insert(key, archives); result = true; } } return result; } void ProjectManager::saveProject(const string& filename) { impl->saveProject(filename); } void ProjectManagerImpl::saveProject(const string& filename) { messageView->putln(); messageView->notify(str(format(_("Saving a project to \"%1%\" ...\n")) % filename)); messageView->flush(); ArchivePtr archive = new Archive(); archive->initSharedInfo(filename); ArchivePtr itemArchive = itemTreeArchiver.store(archive, RootItem::mainInstance()); if(itemArchive){ archive->insert("items", itemArchive); } bool stored = storeObjects(archive, "views", mainWindow->allViews()); stored |= storeObjects(archive, "toolbars", mainWindow->allToolBars()); ArchiverMapMap::iterator p; for(p = archivers.begin(); p != archivers.end(); ++p){ ArchivePtr moduleArchive = new Archive(); moduleArchive->setKeyQuoteStyle(YAML_DOUBLE_QUOTED); ArchiverMap::iterator q; for(q = p->second.begin(); q != p->second.end(); ++q){ ArchivePtr objArchive = new Archive(); objArchive->inheritSharedInfoFrom(archive); ArchiverInfo& info = q->second; if(info.storeFunction(*objArchive)){ const string& objectName = q->first; moduleArchive->insert(objectName, objArchive); } } if(!moduleArchive->empty()){ const string& moduleName = p->first; archive->insert(moduleName, moduleArchive); stored = true; } } if(perspectiveCheck->isChecked()){ MainWindow::instance()->storeLayout(archive); stored = true; } string currentFileDialogDirectory; if(AppConfig::archive()->read("currentFileDialogDirectory", currentFileDialogDirectory)){ archive->writeRelocatablePath("currentFileDialogDirectory", currentFileDialogDirectory); } if(stored){ YamlWriter writer(filename); writer.setKeyOrderPreservationMode(true); writer.putNode(archive); messageView->notify(_("Saving a project file has been finished.\n")); lastAccessedProjectFile = filename; } else { messageView->notify(_("Saving a project file failed.\n")); lastAccessedProjectFile.clear(); } } void ProjectManager::overwriteCurrentProject() { impl->overwriteCurrentProject(); } void ProjectManagerImpl::overwriteCurrentProject() { if(lastAccessedProjectFile.empty()){ openDialogToSaveProject(); } else { saveProject(lastAccessedProjectFile); } } void ProjectManagerImpl::onSigOptionsParsed(boost::program_options::variables_map& v) { if(v.count("project")){ vector projectFileNames = v["project"].as< vector >(); for(size_t i=0; i < projectFileNames.size(); ++i){ loadProject(toActualPathName(projectFileNames[i]), true); } } } void ProjectManagerImpl::openDialogToLoadProject() { QFileDialog dialog(MainWindow::instance()); dialog.setWindowTitle(_("Open a choreonoid project file")); dialog.setFileMode(QFileDialog::ExistingFile); dialog.setViewMode(QFileDialog::List); dialog.setLabelText(QFileDialog::Accept, _("Open")); dialog.setLabelText(QFileDialog::Reject, _("Cancel")); QStringList filters; filters << _("Project files (*.cnoid)"); filters << _("Any files (*)"); dialog.setNameFilters(filters); dialog.setDirectory(AppConfig::archive()->get("currentFileDialogDirectory", App::shareDirectory()).c_str()); if(dialog.exec()){ AppConfig::archive()->writePath("currentFileDialogDirectory", dialog.directory().absolutePath().toStdString()); loadProject(filesystem::path(dialog.selectedFiles().front().toStdString()).file_string(), false); } } void ProjectManagerImpl::openDialogToSaveProject() { QFileDialog dialog(MainWindow::instance()); dialog.setWindowTitle(_("Save a choreonoid project file")); dialog.setFileMode(QFileDialog::AnyFile); dialog.setViewMode(QFileDialog::List); dialog.setLabelText(QFileDialog::Accept, _("Save")); dialog.setLabelText(QFileDialog::Reject, _("Cancel")); QStringList filters; filters << _("Project files (*.cnoid)"); filters << _("Any files (*)"); dialog.setNameFilters(filters); dialog.setDirectory(AppConfig::archive()->get("currentFileDialogDirectory", App::shareDirectory()).c_str()); if(dialog.exec()){ AppConfig::archive()->writePath("currentFileDialogDirectory", dialog.directory().absolutePath().toStdString()); filesystem::path path(dialog.selectedFiles().front().toStdString()); string filename(path.file_string()); string ext = filesystem::extension(path); if(ext != ".cnoid"){ filename += ".cnoid"; } saveProject(filename); } } void ProjectManagerImpl::onPerspectiveCheckToggled() { AppConfig::archive()->openMapping("ProjectManager") ->write("storePerspective", perspectiveCheck->isChecked()); } void ProjectManager::connectArchiver( const std::string& moduleName, const std::string& name, boost::function storeFunction, boost::function restoreFunction) { ProjectManagerImpl::ArchiverInfo& info = impl->archivers[moduleName][name]; info.storeFunction = storeFunction; info.restoreFunction = restoreFunction; } void ProjectManager::disconnectArchivers(const std::string& moduleName) { impl->archivers.erase(moduleName); } choreonoid-1.1.0+dfsg/src/Base/ProjectManager.h000066400000000000000000000022071207742442300213020ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_PROJECT_MANAGER_H_INCLUDED #define CNOID_GUIBASE_PROJECT_MANAGER_H_INCLUDED #include "Archive.h" #include #include #include "exportdecl.h" namespace cnoid { class ExtensionManager; class ProjectManagerImpl; class CNOID_EXPORT ProjectManager { public: static ProjectManager* instance(); void loadProject(const std::string& filename); void saveProject(const std::string& filename); void overwriteCurrentProject(); static void initialize(ExtensionManager* em); private: ProjectManager(ExtensionManager* em); ~ProjectManager(); ProjectManagerImpl* impl; friend class ExtensionManager; friend class ExtensionManagerImpl; void connectArchiver( const std::string& moduleName, const std::string& objectName, boost::function storeFunction, boost::function restoreFunction); void disconnectArchivers(const std::string& moduleName); }; } #endif choreonoid-1.1.0+dfsg/src/Base/ProjectPathSetEditor.cpp000066400000000000000000000125341207742442300230060ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ProjectPathSetEditor.h" #include "AppConfig.h" #include "MainWindow.h" #include "MenuManager.h" #include "Button.h" #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { class PathSetItem : public QTableWidgetItem { public: QString paths; PathSetItem(const QString& paths) : paths(paths) { setFlags(Qt::ItemIsEnabled|Qt::ItemIsEditable); } virtual QVariant data(int role) const { if(role == Qt::DisplayRole || role == Qt::EditRole){ return paths; } return QTableWidgetItem::data(role); } virtual void setData(int role, const QVariant& value) { bool accepted = false; if(role == Qt::EditRole && value.type() == QVariant::String){ paths = value.toString(); } else { QTableWidgetItem::setData(role, value); } } }; class PathSetEditor : public QDialog { public: PathSetEditor(); QTableWidget* tableWidget; QLineEdit* newVariableEntry; void initAndShow(); void readPathSetFromArchive(); void appendPathSet(const QString& name, const QString& paths); void writePathSetToArchive(); void onAppendActivated(); }; } void ProjectPathSetEditor::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ PathSetEditor* editor = new PathSetEditor(); MenuManager& mm = ext->menuManager(); mm.setPath("/File").setPath(N_("Project File Options")); mm.addItem(_("Edit Path Set")) ->sigTriggered().connect(bind(&PathSetEditor::initAndShow, editor)); initialized = true; } } PathSetEditor::PathSetEditor() : QDialog(MainWindow::instance()) { setWindowTitle(_("Project File Path Set")); QVBoxLayout* vbox = new QVBoxLayout(); tableWidget = new QTableWidget(this); tableWidget->setColumnCount(2); tableWidget->setSelectionBehavior(QAbstractItemView::SelectRows); tableWidget->setSelectionMode(QAbstractItemView::NoSelection); tableWidget->setHorizontalHeaderItem(0, new QTableWidgetItem(_("Variable"))); tableWidget->setHorizontalHeaderItem(1, new QTableWidgetItem(_("Path List"))); tableWidget->horizontalHeader()->setStretchLastSection(true); tableWidget->verticalHeader()->hide(); tableWidget->verticalHeader()->setResizeMode(QHeaderView::ResizeToContents); vbox->addWidget(tableWidget, 1); QHBoxLayout* hbox = new QHBoxLayout(); QLabel* label = new QLabel(_("Variable")); hbox->addWidget(label); newVariableEntry = new QLineEdit(); hbox->addWidget(newVariableEntry); PushButton* button = new PushButton(_("Append")); hbox->addWidget(button); button->sigClicked().connect(bind(&PathSetEditor::onAppendActivated, this)); vbox->addLayout(hbox); QPushButton* createButton = new QPushButton(_("&Apply")); createButton->setDefault(true); QPushButton* cancelButton = new QPushButton(_("&Cancel")); QDialogButtonBox* buttonBox = new QDialogButtonBox(this); buttonBox->addButton(createButton, QDialogButtonBox::AcceptRole); buttonBox->addButton(cancelButton, QDialogButtonBox::RejectRole); connect(buttonBox,SIGNAL(accepted()), this, SLOT(accept())); connect(buttonBox,SIGNAL(rejected()), this, SLOT(reject())); vbox->addWidget(buttonBox); setLayout(vbox); } void PathSetEditor::initAndShow() { readPathSetFromArchive(); if(exec() == QDialog::Accepted){ writePathSetToArchive(); } } void PathSetEditor::readPathSetFromArchive() { YamlMappingPtr pathSets = AppConfig::archive()->openMapping("ArchivePathSet"); tableWidget->setRowCount(0); for(YamlMapping::const_iterator p = pathSets->begin(); p != pathSets->end(); ++p){ const std::string& name = p->first; YamlNodePtr value = p->second; if(!name.empty() && value->isString()){ appendPathSet(name.c_str(), value->toString().c_str()); } } } void PathSetEditor::appendPathSet(const QString& name, const QString& paths) { int row = tableWidget->rowCount(); tableWidget->setRowCount(row + 1); QTableWidgetItem* nameItem = new QTableWidgetItem(name); nameItem->setFlags(Qt::ItemIsEnabled); tableWidget->setItem(row, 0, nameItem); PathSetItem* pathSetItem = new PathSetItem(paths); tableWidget->setItem(row, 1, pathSetItem); } void PathSetEditor::writePathSetToArchive() { YamlMappingPtr pathSets = AppConfig::archive()->openMapping("ArchivePathSet"); pathSets->clear(); int n = tableWidget->rowCount(); for(int i=0; i < n; ++i){ PathSetItem* item = dynamic_cast(tableWidget->item(i, 1)); if(item){ string name = tableWidget->item(i, 0)->text().toStdString(); if(!name.empty() && !item->paths.isEmpty()){ pathSets->write(name, item->paths.toStdString()); } } } } void PathSetEditor::onAppendActivated() { appendPathSet(newVariableEntry->text(), ""); } choreonoid-1.1.0+dfsg/src/Base/ProjectPathSetEditor.h000066400000000000000000000005071207742442300224500ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_PROJECT_PATH_SET_EDITOR_H_INCLUDED #define CNOID_GUIBASE_PROJECT_PATH_SET_EDITOR_H_INCLUDED namespace cnoid { class ExtensionManager; class ProjectPathSetEditor { public: static void initialize(ExtensionManager* ext); }; } #endif choreonoid-1.1.0+dfsg/src/Base/PutPropertyFunction.h000066400000000000000000000034411207742442300224250ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_PUT_PROPERTY_FUNCTION_H_INCLUDED #define CNOID_GUIBASE_PUT_PROPERTY_FUNCTION_H_INCLUDED #include #include #include namespace cnoid { class Selection { public: inline Selection(std::vector labels, int which = 0) : labels(labels), which(which) { } inline std::string label() const { return labels[which]; } std::vector labels; int which; }; class PutPropertyFunction { public: virtual ~PutPropertyFunction() { } // bool virtual void operator()(const std::string& name, bool value) = 0; virtual void operator()(const std::string& name, bool value, const boost::function& changeFunc) = 0; // int virtual void operator()(const std::string& name, int value) = 0; virtual void operator()(const std::string& name, int value, const boost::function& changeFunc) = 0; // double virtual void operator()(const std::string& name, double value) = 0; virtual void operator()(const std::string& name, double value, const boost::function& changeFunc) = 0; // string virtual void operator()(const std::string& name, const std::string& value) = 0; virtual void operator()(const std::string& name, const std::string& value, const boost::function& changeFunc) = 0; // selection virtual void operator()(const std::string& name, Selection selection) = 0; virtual void operator()(const std::string& name, Selection selection, const boost::function& changeFunc) = 0; }; } #endif choreonoid-1.1.0+dfsg/src/Base/RootItem.cpp000066400000000000000000000110211207742442300204700ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "RootItem.h" #include "ExtensionManager.h" #include "ItemManager.h" #include "LazySignal.h" #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } namespace cnoid { class RootItemImpl { public: RootItem* self; RootItemImpl(RootItem* self); ~RootItemImpl(); boost::signal sigDestroyed; boost::signal sigItemAdded; boost::signal sigItemRemoving; boost::signal sigItemRemoved; LazySignal< boost::signal > sigTreeChanged; }; } void RootItem::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ext->itemManager().registerClass(N_("RootItem")); ext->manage(RootItemPtr(mainInstance())); initialized = true; } } RootItem* RootItem::mainInstance() { static RootItem* rootItem = new RootItem("Root"); return rootItem; } RootItem::RootItem() { initializeInstance(); } RootItem::RootItem(const std::string& name) : Item(name) { initializeInstance(); } RootItem::RootItem(const RootItem& org) : Item(org) { initializeInstance(); } void RootItem::initializeInstance() { impl = new RootItemImpl(this); unsetAttribute(NAME_EDITABLE); unsetAttribute(MOVABLE); unsetAttribute(CHECKABLE); } RootItemImpl::RootItemImpl(RootItem* self) : self(self) { } RootItem::~RootItem() { delete impl; } RootItemImpl::~RootItemImpl() { if(TRACE_FUNCTIONS){ cout << "RootItemImpl::~RootItemImpl()" << endl; } sigDestroyed(self); } SignalProxy< boost::signal > RootItem::sigDestroyed() { return impl->sigDestroyed; } SignalProxy< boost::signal > RootItem::sigItemAdded() { return impl->sigItemAdded; } /** @if jp 本シグナルを所有ã™ã‚‹ãƒ«ãƒ¼ãƒˆã‚¢ã‚¤ãƒ†ãƒ ã‹ã‚‰ã®ãƒ‘スã«æ‰€å±žã™ã‚‹å­ã‚¢ã‚¤ãƒ†ãƒ ãŒãƒ‘スã‹ã‚‰ å–り除ã‹ã‚Œã‚‹ç›´å‰ã«ç™ºè¡Œã•ã‚Œã‚‹ã‚·ã‚°ãƒŠãƒ«ã€‚ @param isMoving アイテムãŒç§»å‹•ä¸­ã§ã‚ã£ã¦ã€å†ã³æœ¬ãƒ«ãƒ¼ãƒˆã‚¢ã‚¤ãƒ†ãƒ ã‹ã‚‰ã®ãƒ‘ス㫠所属ã™ã‚‹å ´åˆã€true ã¨ãªã‚‹ã€‚ @todo ã§ãã‚Œã°æœ¬ã‚·ã‚°ãƒŠãƒ«ã¯ itemRemoved() シグナルã§ç½®ãæ›ãˆã¦deprecatedã¨ã—ãŸã„。 */ SignalProxy< boost::signal > RootItem::sigItemRemoving() { return impl->sigItemRemoving; } /** @if jp 本シグナルを所有ã™ã‚‹ãƒ«ãƒ¼ãƒˆã‚¢ã‚¤ãƒ†ãƒ ã‹ã‚‰ã®ãƒ‘スã«æ‰€å±žã™ã‚‹å­ã‚¢ã‚¤ãƒ†ãƒ ãŒãƒ‘スã‹ã‚‰ å–り除ã‹ã‚ŒãŸå¾Œã«å‘¼ã°ã‚Œã‚‹ã‚¹ãƒ­ãƒƒãƒˆã‚’接続ã™ã‚‹ã€‚ @param isMoving アイテムãŒç§»å‹•ä¸­ã§ã‚ã£ã¦ã€å†ã³æœ¬ãƒ«ãƒ¼ãƒˆã‚¢ã‚¤ãƒ†ãƒ ã‹ã‚‰ã®ãƒ‘ス㫠所属ã™ã‚‹å ´åˆã€true ã¨ãªã‚‹ã€‚ */ SignalProxy< boost::signal > RootItem::sigItemRemoved() { return impl->sigItemRemoved; } /** @if jp アイテムã®è¿½åŠ ãƒ»å‰Šé™¤ãªã©ã€ã‚¢ã‚¤ãƒ†ãƒ ãƒ„リーã®æ§‹é€ ãŒå¤‰åŒ–ã—ãŸã¨ãã«å‘¼ã°ã‚Œã‚‹ã‚¹ãƒ­ãƒƒãƒˆã‚’接続ã™ã‚‹ã€‚ sigItemAdded ã‚„ sigItemRemoving ã¨ã¯ç•°ãªã‚Šã€ä¸€åº¦ã«è¡Œã‚れる一連ã®æ“作ã«å¯¾ã—㦠１回ã ã‘ã¾ã¨ã‚ã¦ç™ºè¡Œã•ã‚Œã‚‹ã€‚正確ã«ã¯ã€ãƒ•ãƒ¬ãƒ¼ãƒ ãƒ¯ãƒ¼ã‚¯ã®ã‚¤ãƒ™ãƒ³ãƒˆãƒ«ãƒ¼ãƒ—ã§ã‚­ãƒ¥ãƒ¼ã«ã‚るイベント ãŒå‡¦ç†ã•ã‚Œã¦ã‹ã‚‰å®Ÿè¡Œã•ã‚Œã‚‹ã€‚ @todo 「1回ã ã‘ã¾ã¨ã‚ã¦ã€ã¯æらãプロジェクト読ã¿è¾¼ã¿æ™‚ãªã©ã«ã¯å®ˆã‚‰ã‚Œã¦ã„ãªã„ã®ã§ï¼Œ ã“ã®ç‚¹æ”¹å–„ã—ã¦ãŠã． @endif */ SignalProxy< boost::signal > RootItem::sigTreeChanged() { return impl->sigTreeChanged.signal(); } void RootItem::notifyEventOnItemAdded(Item* item) { if(TRACE_FUNCTIONS){ cout << "RootItem::notifyEventOnItemAdded()" << endl; } impl->sigItemAdded(item); impl->sigTreeChanged.request(); } void RootItem::notifyEventOnItemRemoving(Item* item, bool isMoving) { impl->sigItemRemoving(item, isMoving); impl->sigTreeChanged.request(); } void RootItem::notifyEventOnItemRemoved(Item* item, bool isMoving) { impl->sigItemRemoved(item, isMoving); impl->sigTreeChanged.request(); } ItemPtr RootItem::doDuplicate() const { return new RootItem(*this); } bool RootItem::store(Archive& archive) { return true; } bool RootItem::restore(const Archive& archive) { return true; } choreonoid-1.1.0+dfsg/src/Base/RootItem.h000066400000000000000000000031351207742442300201440ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_ROOT_ITEM_H_INCLUDED #define CNOID_GUIBASE_ROOT_ITEM_H_INCLUDED #include "Item.h" #include "ItemList.h" #include "exportdecl.h" namespace cnoid { class Project; class Item; class RootItemImpl; /** @if jp アイテムã®æœ¨æ§‹é€ ã®ãƒ«ãƒ¼ãƒˆã¨ãªã‚‹ã‚¯ãƒ©ã‚¹ã€‚ @endif */ class CNOID_EXPORT RootItem : public Item { public: static void initialize(ExtensionManager* ext); static RootItem* mainInstance(); RootItem(); RootItem(const std::string& name); RootItem(const RootItem& org); virtual ~RootItem(); SignalProxy< boost::signal > sigDestroyed(); SignalProxy< boost::signal > sigItemAdded(); SignalProxy< boost::signal > sigItemRemoving(); SignalProxy< boost::signal > sigItemRemoved(); SignalProxy< boost::signal > sigTreeChanged(); protected: virtual ItemPtr doDuplicate() const; virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); private: void initializeInstance(); friend class Item; void notifyEventOnItemAdded(Item* item); void notifyEventOnItemRemoving(Item* item, bool isMoving); void notifyEventOnItemRemoved(Item* item, bool isMoving); friend class RootItemImpl; RootItemImpl* impl; }; typedef boost::intrusive_ptr RootItemPtr; } #endif choreonoid-1.1.0+dfsg/src/Base/SceneGL.cpp000066400000000000000000000037541207742442300202240ustar00rootroot00000000000000/** \author Shin'ichiro Nakaoka */ #include "SceneGL.h" #include using namespace osg; using namespace cnoid; namespace cnoid { class SceneGLDrawable : public osg::Drawable { public: SceneGLDrawable() { owner = 0; } SceneGLDrawable(const SceneGLDrawable& org, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY) : osg::Drawable(org, copyop) { owner = 0; } META_Object(cnoidBase, SceneGLDrawable) virtual ~SceneGLDrawable() { } virtual void drawImplementation(osg::RenderInfo& ri) const { owner->drawImplementation(ri); } virtual osg::BoundingBox computeBound() const { return owner->computeGLBound(); } virtual void accept(osg::PrimitiveFunctor& functor) const { owner->accept(functor); } SceneGL* owner; }; } SceneGL::SceneGL() { initializeDrawable(new SceneGLDrawable()); } SceneGL::SceneGL(const SceneGL& org, const osg::CopyOp& copyop) : SceneObject(org, copyop) { initializeDrawable(new SceneGLDrawable(*org.drawable, copyop)); } void SceneGL::initializeDrawable(SceneGLDrawable* drawable) { this->drawable = drawable; drawable->owner = this; osg::Geode* geode = new osg::Geode(); geode->addDrawable(drawable); addChild(geode); } SceneGL::~SceneGL() { } void SceneGL::setUseDisplayList(bool on) { drawable->setUseDisplayList(on); } void SceneGL::dirtyDisplayList() { drawable->dirtyDisplayList(); drawable->dirtyBound(); requestRedraw(); } osg::BoundingBox SceneGL::computeGLBound() const { return osg::BoundingBox(); } /** Reimplement this function and specify vertices using the functor object when you need the intersection test on the objects rendered by custom drawImplementation(). */ void SceneGL::accept(osg::PrimitiveFunctor& functor) const { drawable->osg::Drawable::accept(functor); } choreonoid-1.1.0+dfsg/src/Base/SceneGL.h000066400000000000000000000020631207742442300176610ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_SCENE_GL_H_INCLUDED #define CNOID_GUIBASE_SCENE_GL_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class SceneGLDrawable; /** This class is used for drawing a 3D object with raw OpenGL commands. */ class CNOID_EXPORT SceneGL : public SceneObject { public: SceneGL(); SceneGL(const SceneGL& org, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY); virtual ~SceneGL(); protected: void setUseDisplayList(bool on); void dirtyDisplayList(); virtual void drawImplementation(osg::RenderInfo& ri) const = 0; virtual osg::BoundingBox computeGLBound() const; virtual void accept(osg::PrimitiveFunctor& functor) const; private: void initializeDrawable(SceneGLDrawable* drawable); osg::ref_ptr drawable; friend class SceneGLDrawable; }; typedef osg::ref_ptr SceneGLPtr; } #endif choreonoid-1.1.0+dfsg/src/Base/SceneItem.cpp000066400000000000000000000114471207742442300206160ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "SceneItem.h" #include "SceneView.h" #include "ItemTreeView.h" #include "ItemManager.h" #include "Archive.h" #include "RootItem.h" #include "OptionManager.h" #include "PutPropertyFunction.h" #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; bool loadSceneItem(SceneItem* item, const std::string& filename, std::ostream& os) { SceneObject::ReadResult result = item->loadScene(filename); if(result.success()){ if(item->name().empty()){ item->setName(filesystem::path(filename).leaf()); } } else { os << result.message(); } return result.success(); } class SceneItemManager { SceneView* sceneView; std::set visibleSceneItems; public: SceneItemManager(ExtensionManager* ext) { ext->itemManager().registerClass("SceneItem"); ext->itemManager().addLoader( _("Scene file"), "OSG-IMPORTABLE-SCENE-FILE", "", bind(loadSceneItem, _1, _2, _3), ItemManager::PRIORITY_CONVERSION); ext->optionManager().addOption("model", program_options::value< vector >(), "load a 3d model file"); ext->optionManager().sigOptionsParsed().connect( bind(&SceneItemManager::onSigOptionsParsed, this, _1)); ItemTreeView::mainInstance()->sigCheckToggled().connect( bind(&SceneItemManager::onItemCheckToggled, this, _1, _2)); sceneView = SceneView::mainInstance(); } void onSigOptionsParsed(boost::program_options::variables_map& variables) { if(variables.count("model")){ vector files = variables["model"].as< vector >(); for(size_t i=0; i < files.size(); ++i){ SceneItemPtr item(new SceneItem()); if(item->load(files[i], "OSG-IMPORTABLE-SCENE-FILE")){ RootItem::mainInstance()->addChildItem(item); ItemTreeView::mainInstance()->checkItem(item, true); } } } } void onItemCheckToggled(Item* item, bool isChecked) { SceneItem* sceneItem = dynamic_cast(item); if(sceneItem){ std::set::iterator p = visibleSceneItems.find(sceneItem); if(isChecked){ if(p == visibleSceneItems.end()){ sceneView->addSceneObject(sceneItem->sceneObject()); sceneView->requestRedraw(); visibleSceneItems.insert(sceneItem); } } else { if(p != visibleSceneItems.end()){ sceneView->removeSceneObject(sceneItem->sceneObject()); sceneView->requestRedraw(); visibleSceneItems.erase(p); } } } } ~SceneItemManager() { std::set::iterator p; for(p = visibleSceneItems.begin(); p != visibleSceneItems.end(); ++p){ SceneItemPtr sceneItem = *p; sceneView->removeSceneObject(sceneItem->sceneObject()); } } }; } void SceneItem::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ext->manage(new SceneItemManager(ext)); initialized = true; } } SceneItem::SceneItem() : sceneObject_(new SceneObject) { } SceneItem::SceneItem(const SceneItem& org) : Item(org), sceneObject_(new SceneObject(*org.sceneObject_)) { } SceneItem::~SceneItem() { } SceneObject::ReadResult SceneItem::loadScene(const std::string& filename) { return sceneObject_->load(filename); } ItemPtr SceneItem::doDuplicate() const { return new SceneItem(*this); } void SceneItem::doPutProperties(PutPropertyFunction& putProperty) { putProperty(_("Scene file"), filesystem::path(lastAccessedFileName()).leaf()); } bool SceneItem::store(Archive& archive) { if(!lastAccessedFileName().empty()){ archive.writeRelocatablePath("file", lastAccessedFileName()); archive.write("format", lastAccessedFileFormatId()); } return true; } bool SceneItem::restore(const Archive& archive) { std::string filename, formatId; if(archive.readRelocatablePath("file", filename) && archive.read("format", formatId)){ if(load(filename, archive.currentParentItem(), formatId)){ return true; } } return false; } choreonoid-1.1.0+dfsg/src/Base/SceneItem.h000066400000000000000000000017141207742442300202570ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_SCENE_ITEM_H_INCLUDED #define CNOID_GUIBASE_SCENE_ITEM_H_INCLUDED #include "Item.h" #include "SceneObject.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT SceneItem : public Item { public: static void initialize(ExtensionManager* ext); SceneItem(); SceneItem(const SceneItem& org); virtual ~SceneItem(); inline SceneObject* sceneObject() { return sceneObject_.get(); } SceneObject::ReadResult loadScene(const std::string& filename); protected: virtual ItemPtr doDuplicate() const; virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); virtual void doPutProperties(PutPropertyFunction& putProperty); private: SceneObjectPtr sceneObject_; }; typedef boost::intrusive_ptr SceneItemPtr; } #endif choreonoid-1.1.0+dfsg/src/Base/SceneObject.cpp000066400000000000000000000065101207742442300211210ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "SceneObject.h" #include "SceneView.h" #include "MessageView.h" #include using namespace cnoid; SceneViewEvent::SceneViewEvent() : point_(0.0, 0.0, 0.0) { x_ = 0.0; y_ = 0.0; key_ = 0; modKeyMask_ = 0; button_ = 0; scrollingMotion_ = osgGA::GUIEventAdapter::SCROLL_NONE; scrollingDelta_ = 0.0; } SceneViewEvent::SceneViewEvent(const SceneViewEvent& org) : camera_(org.camera_), point_(org.point_), path_(org.path_) { x_ = org.x_; y_ = org.y_; key_ = org.key_; modKeyMask_ = org.modKeyMask_; button_ = org.button_; scrollingMotion_ = org.scrollingMotion_; scrollingDelta_ = org.scrollingDelta_; } void SceneViewEvent::updateIndicator(const std::string& text) const { sceneView->updateIndicator(text); } SceneObject::SceneObject() { isActive_ = false; isEditable_ = false; sceneMode_ = VIEW_MODE; } SceneObject::SceneObject(const SceneObject& org, const osg::CopyOp& copyop) : osg::Group(org), isEditable_(org.isEditable_), sceneMode_(org.sceneMode_) { isActive_ = false; } SceneObject::~SceneObject() { if(isActive_){ onDetachedFromScene(); } } const char* SceneObject::libraryName() const { return "cnoid"; } const char* SceneObject::className() const { return "SceneObject"; } SceneObject::ReadResult SceneObject::load(const std::string filename) { MessageView::mainInstance()->beginStdioRedirect(); /** Probably error message can be obtained by using osgDB::Registry class. First, get a ReaderWriter object with getReaderWriterForExtension(), and use it to load a file. Messages can be obtained from ReadResult object. Let's see the implementation of osgDB::readNodeFile(), too. */ osg::Node* node = osgDB::readNodeFile(filename); if(node){ addChild(node); return osgDB::ReaderWriter::ReadResult(osgDB::ReaderWriter::ReadResult::FILE_LOADED); } return osgDB::ReaderWriter::ReadResult(osgDB::ReaderWriter::ReadResult::ERROR_IN_READING_FILE); MessageView::mainInstance()->endStdioRedirect(); } void SceneObject::setEditable(bool on) { isEditable_ = on; } bool SceneObject::isEditable() { return isEditable_; } void SceneObject::onAttachedToScene() { } void SceneObject::onDetachedFromScene() { } bool SceneObject::onKeyPressEvent(const SceneViewEvent& event) { return false; } bool SceneObject::onKeyReleaseEvent(const SceneViewEvent& event) { return false; } bool SceneObject::onButtonPressEvent(const SceneViewEvent& event) { return false; } bool SceneObject::onButtonReleaseEvent(const SceneViewEvent& event) { return false; } bool SceneObject::onDoubleClickEvent(const SceneViewEvent& event) { return false; } bool SceneObject::onPointerMoveEvent(const SceneViewEvent& event) { return false; } void SceneObject::onPointerLeaveEvent(const SceneViewEvent& event) { } bool SceneObject::onScrollEvent(const SceneViewEvent& event) { return false; } void SceneObject::onSceneModeChanged() { } bool SceneObject::onUndoRequest() { return false; } bool SceneObject::onRedoRequest() { return false; } void SceneObject::onContextMenuRequest(const SceneViewEvent& event, MenuManager& menuManager) { } choreonoid-1.1.0+dfsg/src/Base/SceneObject.h000066400000000000000000000106231207742442300205660ustar00rootroot00000000000000;/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_SCENE_OBJECT_H_INCLUDED #define CNOID_GUIBASE_SCENE_OBJECT_H_INCLUDED #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class MenuManager; class SceneView; class SceneViewImpl; class CNOID_EXPORT SceneViewEvent { public: SceneViewEvent(const SceneViewEvent& org); inline const osg::Vec3d& point() const { return point_; } inline const osg::Vec3d& normal() const { return normal_; } inline const osg::NodePath& path() const { return path_; } inline osg::Camera* camera() const { return camera_.get(); } inline double x() const { return x_; } /** The value is increasing from the bottom to top (the coordinate of the OpenGL viewport). */ inline double y() const { return y_; } /** @return a osgGA::GUIEventAdapter::KeySymbol value */ inline int key() const { return key_; } /** @return a osgGA::GUIEventAdapter::MouseButtonMask value */ inline int button() const { return button_; } /** @return a osgGA::GUIEventAdapter::ModKeyMask value */ inline int modKeyMask() const { return modKeyMask_; } /** @return a osgGA::GUIEventAdapter::ScrollingMotion value */ inline int scrollingMotion() const { return scrollingMotion_; } inline double scrollingDelta() const { return scrollingDelta_; } void updateIndicator(const std::string& message) const; private: osg::ref_ptr camera_; osg::Vec3d point_; osg::Vec3d normal_; osg::NodePath path_; double x_; double y_; int key_; int modKeyMask_; int button_; int scrollingMotion_; double scrollingDelta_; SceneView* sceneView; SceneViewEvent(); SceneViewEvent& operator=(const SceneViewEvent& org); // disabled friend class SceneViewImpl; }; class CNOID_EXPORT SceneObject : public osg::Group { public: SceneObject(); SceneObject(const SceneObject& org, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY); virtual const char* libraryName() const; virtual const char* className () const; typedef osgDB::ReaderWriter::ReadResult ReadResult; ReadResult load(const std::string filename); inline bool isActive() { return isActive_; } // This should be protected but VC++ requires it to be public. enum SceneMode { VIEW_MODE, EDIT_MODE }; void setEditable(bool on); bool isEditable(); protected: inline SceneMode sceneMode() { return sceneMode_; } inline void requestRedraw(){ sigRedrawRequest(0); } inline void requestRedrawWhenEffectNodeCreated(){ sigRedrawRequest(EFFECT_NODE_CREATED); } virtual ~SceneObject(); virtual void onAttachedToScene(); virtual void onDetachedFromScene(); virtual bool onKeyPressEvent(const SceneViewEvent& event); virtual bool onKeyReleaseEvent(const SceneViewEvent& event); virtual bool onButtonPressEvent(const SceneViewEvent& event); virtual bool onButtonReleaseEvent(const SceneViewEvent& event); virtual bool onDoubleClickEvent(const SceneViewEvent& event); virtual bool onPointerMoveEvent(const SceneViewEvent& event); virtual void onPointerLeaveEvent(const SceneViewEvent& event); virtual bool onScrollEvent(const SceneViewEvent& event); virtual void onContextMenuRequest(const SceneViewEvent& event, MenuManager& menuManager); virtual void onSceneModeChanged(); virtual bool onUndoRequest(); virtual bool onRedoRequest(); private: bool isActive_; // in the scene graph ? bool isEditable_; SceneMode sceneMode_; enum RedrawRequestFlag { EFFECT_NODE_CREATED = 1 }; /// \todo remove this signal and do the same thing with more simple, efficient way boost::signal sigRedrawRequest; friend class SceneViewImpl; }; typedef osg::ref_ptr SceneObjectPtr; } #endif choreonoid-1.1.0+dfsg/src/Base/ScenePieces.cpp000066400000000000000000000204341207742442300211240ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ScenePieces.h" #include #include #include #include #include #include using namespace std; using namespace cnoid; namespace { osg::Geometry* createCrossGeometry(const osg::Vec4& color, float size, float lineWidth) { osg::Geometry* geom = new osg::Geometry; osg::StateSet* state = geom->getOrCreateStateSet(); state->setMode(GL_LIGHTING, osg::StateAttribute::OFF); //state->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON); //state->setMode(GL_BLEND, osg::StateAttribute::ON); osg::Material* mat = new osg::Material; mat->setDiffuse(osg::Material::FRONT_AND_BACK, color); state->setAttribute(mat); if(lineWidth != 1.0f){ state->setAttribute(new osg::LineWidth(lineWidth)); } osg::Vec3Array* v = new osg::Vec3Array; geom->setVertexArray(v); v->push_back(osg::Vec3(-size, 0.0f, 0.0f)); v->push_back(osg::Vec3( size, 0.0f, 0.0f)); v->push_back(osg::Vec3( 0.0f, -size, 0.0f)); v->push_back(osg::Vec3( 0.0f, size, 0.0f)); v->push_back(osg::Vec3( 0.0f, 0.0f, -size)); v->push_back(osg::Vec3( 0.0f, 0.0f, size)); geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, v->size())); return geom; } } SceneMarker::~SceneMarker() { } void SceneMarker::setPosition(const Vector3& p) { setMatrix(osg::Matrix(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, p(0), p(1), p(2), 1.0)); } CrossMarker::CrossMarker(const osg::Vec4& color, float size, float lineWidth) : color(color), lineWidth(lineWidth) { geode = new osg::Geode; geode->addDrawable(createCrossGeometry(color, size, lineWidth)); geode->setDataVariance(osg::Object::STATIC); addChild(geode.get()); } void CrossMarker::setSize(float size) { geode->removeDrawables(0); geode->addDrawable(createCrossGeometry(color, size, lineWidth)); } SphereMarker::SphereMarker(float radius, const osg::Vec4& color) { sphere = new osg::Sphere; sphere->setRadius(radius); osg::ShapeDrawable* drawable = new osg::ShapeDrawable(sphere.get()); osg::Geode* geode = new osg::Geode; geode->addDrawable(drawable); osg::StateSet* ss = drawable->getOrCreateStateSet(); osg::Material* material = new osg::Material; material->setDiffuse(osg::Material::FRONT_AND_BACK, color); material->setEmission(osg::Material::FRONT_AND_BACK, color); ss->setAttribute(material, osg::StateAttribute::OVERRIDE); // The code to enable alpha blending ss->setMode(GL_BLEND, osg::StateAttribute::ON); ss->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); ss->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON); osg::Depth* depth = new osg::Depth; depth->setWriteMask(false); ss->setAttributeAndModes(depth, osg::StateAttribute::ON); geode->setDataVariance(osg::Object::STATIC); addChild(geode); } void SphereMarker::setRadius(float r) { sphere->setRadius(r); } void SphereMarker::setCross(const osg::Vec4& color, float size, float lineWidth) { osg::Geode* geode = dynamic_cast(getChild(0)); if(geode){ if(cross){ geode->removeDrawable(cross.get()); } cross = createCrossGeometry(color, size, lineWidth); geode->addDrawable(cross); } } BBMarker::BBMarker(const osg::BoundingBox& bbox, const osg::Vec4& color, float width) { create(bbox, color, width); } BBMarker::BBMarker(const osg::BoundingBox& bbox, const osg::Vec4& color) { double h = bbox.xMax() - bbox.xMin(); double w = bbox.yMax() - bbox.yMin(); double d = bbox.zMax() - bbox.zMin(); double width = (h + w + d) / 3.0 / 10.0; create(bbox, color, width); } void BBMarker::create(const osg::BoundingBox& bbox, const osg::Vec4& color, float width) { const double& x0 = bbox.xMin(); const double& x1 = bbox.xMax(); const double& y0 = bbox.yMin(); const double& y1 = bbox.yMax(); const double& z0 = bbox.zMin(); const double& z1 = bbox.zMax(); addMarker(x0, y0, z0, width); addMarker(x0, y0, z1, width); addMarker(x0, y1, z0, width); addMarker(x0, y1, z1, width); addMarker(x1, y0, z0, width); addMarker(x1, y0, z1, width); addMarker(x1, y1, z0, width); addMarker(x1, y1, z1, width); osg::StateSet* ss = getOrCreateStateSet(); osg::Material* material = new osg::Material; material->setDiffuse(osg::Material::FRONT_AND_BACK, color); material->setEmission(osg::Material::FRONT_AND_BACK, color); ss->setAttribute(material, osg::StateAttribute::OVERRIDE); // The code to enable alpha blending ss->setMode(GL_BLEND, osg::StateAttribute::ON); ss->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); ss->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON); osg::Depth* depth = new osg::Depth; depth->setWriteMask(false); ss->setAttributeAndModes(depth, osg::StateAttribute::ON); setDataVariance(osg::Object::STATIC); } void BBMarker::addMarker(double x, double y, double z, float width) { addDrawable(new osg::ShapeDrawable(new osg::Box(osg::Vec3(x, y, z), width))); } AttitudeDragger::AttitudeDragger() { currentParent = 0; static const int numDivisions = 30; static const float beltWidthRatio = 0.1f; static const int axes[3][3] = { { 0, 1, 2 }, { 1, 0, 2 }, { 2, 1, 0 } }; static const char* axisNames[3] = { "x", "y", "z" }; // make dragger belts for(int i=0; i < 3; ++i){ const int x = axes[i][0]; const int y = axes[i][1]; const int z = axes[i][2]; osg::Geometry* geometry = new osg::Geometry(); osg::Vec3Array* vertices = new osg::Vec3Array(); osg::DrawElementsUShort* face = new osg::DrawElementsUShort(osg::PrimitiveSet::QUADS, 0); for(int j=0; j < numDivisions; ++j){ static const double PI = 3.14159265358979323846; const double theta = j * 2.0 * PI / numDivisions; osg::Vec3 v1, v2; v1[x] = beltWidthRatio; v2[x] = -beltWidthRatio; v1[y] = v2[y] = cos(theta); v1[z] = v2[z] = sin(theta); vertices->push_back(v1); vertices->push_back(v2); const int s = j * 2; const int t = ((j + 1) % numDivisions) * 2; face->push_back(s); face->push_back(t); face->push_back(t + 1); face->push_back(s + 1); } geometry->setVertexArray(vertices); geometry->addPrimitiveSet(face); osg::Geode* geode = new osg::Geode; geode->setName(axisNames[i]); geode->addDrawable(geometry); osg::StateSet* state = geode->getOrCreateStateSet(); osg::Material* mat = new osg::Material; osg::Vec4 color(0.2f, 0.2f, 0.2f, 0.4f); color[i] = 1.0f; mat->setEmission(osg::Material::FRONT_AND_BACK, color); mat->setDiffuse(osg::Material::FRONT_AND_BACK, osg::Vec4(0.0f, 0.0f, 0.0f, 0.4f)); state->setAttribute(mat); geode->setDataVariance(osg::Object::STATIC); addChild(geode); draggers[i] = geode; } osg::StateSet* state = getOrCreateStateSet(); osg::LightModel* lightModel = new osg::LightModel; lightModel->setTwoSided(true); state->setAttributeAndModes(lightModel); osg::Material* material = new osg::Material; state->setAttribute(material); state->setMode(GL_BLEND, osg::StateAttribute::ON); state->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); state->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON); osg::Depth* depth = new osg::Depth; depth->setWriteMask(false); state->setAttributeAndModes(depth, osg::StateAttribute::ON); state->setDataVariance(osg::Object::STATIC); } void AttitudeDragger::attachTo(osg::Group* parent) { if(currentParent){ currentParent->removeChild(this); currentParent = 0; } if(parent){ radius_ = parent->getBound().radius(); osg::Matrix S; S.makeScale(radius_, radius_, radius_); setMatrix(S); parent->addChild(this); currentParent = parent; } } choreonoid-1.1.0+dfsg/src/Base/ScenePieces.h000066400000000000000000000036201207742442300205670ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_SCENE_PIECES_H_INCLUDED #define CNOID_GUIBASE_SCENE_PIECES_H_INCLUDED #include #include #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT SceneMarker : public osg::MatrixTransform { public: virtual ~SceneMarker(); void setPosition(const Vector3& p); }; class CNOID_EXPORT CrossMarker : public SceneMarker { public: CrossMarker(const osg::Vec4& color, float size, float lineWidth = 1.0f); void setSize(float size); private: osg::ref_ptr geode; osg::Vec4 color; float lineWidth; }; class CNOID_EXPORT SphereMarker : public SceneMarker { public: SphereMarker(float radius, const osg::Vec4& color); void setRadius(float radius); void setCross(const osg::Vec4& color, float size, float lineWidth = 1.0f); private: osg::ref_ptr sphere; osg::ref_ptr cross; }; class CNOID_EXPORT BBMarker : public osg::Geode { public: BBMarker(const osg::BoundingBox& bbox, const osg::Vec4& color, float width); BBMarker(const osg::BoundingBox& bbox, const osg::Vec4& color); private: void create(const osg::BoundingBox& bbox, const osg::Vec4& color, float width); void addMarker(double x, double y, double z, float width); }; class CNOID_EXPORT AttitudeDragger : public osg::MatrixTransform { public: AttitudeDragger(); void attachTo(osg::Group* parent); inline void detach() { attachTo(0); } inline float radius() { return radius_; } private: osg::ref_ptr draggers[3]; osg::Group* currentParent; float radius_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/SceneView.cpp000066400000000000000000001661641207742442300206410ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "SceneView.h" #include "SceneObject.h" #include "OsgViewer.h" #include "ToolBar.h" #include "MessageView.h" #include "MenuManager.h" #include "Archive.h" #include "MainWindow.h" #include "SpinBox.h" #include "Separator.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { bool TRACE_FUNCTIONS = false; SceneView* mainSceneView = 0; const bool ENABLE_PERSPECTIVE_COORDINATE_AXES = false; class SetupDialog : public QDialog { public: DoubleSpinBox floorGridSpanSpin; DoubleSpinBox floorGridIntervalSpin; CheckBox hiPriorityRenderingCheck; SetupDialog(SceneViewImpl* impl); void storeState(Archive& archive); void restoreState(const Archive& archive); }; } namespace cnoid { class SceneViewImpl : public QObject, public boost::signals::trackable { public: SceneViewImpl(SceneView* self); ~SceneViewImpl(); SceneView* self; SetupDialog setup; OsgViewer* viewer; int drawingAreaHeight; osg::ref_ptr camera; int numOverriddenCameraControls; enum ProjectionMode { PERSPECTIVE, ORTHO } projectionMode; osg::Matrixd orgOrthoProjectionMatrix; osg::Matrixd orgPerspectiveProjectionMatrix; osg::ref_ptr hudCamera; osg::ref_ptr axesTransform; bool coordinateAxesEnabled; osg::ref_ptr rootNode; osg::ref_ptr objectGroupNode; osg::ref_ptr switchNode; osg::ref_ptr floorGridNode; osg::ref_ptr polygonMode; osg::ref_ptr scribe; osg::ref_ptr cartoon; osg::ref_ptr shadowedScene; osg::ref_ptr shadowMap; osg::ref_ptr preprocessingNodeGroup; ostream& os; QLabel* indicatorLabel; ToolButton* editModeToggle; ToolButton* perspectiveToggle; ToolButton* floorGridToggle; ToolButton* collisionToggle; Action* shadowCheck; QString lastCaptureFile; QString lastCaptureFolder; Menu* popupMenu; MenuManager menuManager; SceneObject::SceneMode operationMode; enum DragMode { NO_DRAGGING, OBJECT_DELEGATION, VIEW_ROTATION, VIEW_TRANSLATION, VIEW_ZOOM } dragMode; osg::Matrixd homeViewMatrix; double orgMouseX; double orgMouseY; osg::Matrixd orgViewMatrix; osg::Vec3d orgViewUp; osg::Vec3d orgViewSide; osg::Vec3d orgEye; osg::Vec3d orgViewForward; double viewTranslationRatioX; double viewTranslationRatioY; double orgZoomDistance; SceneViewEvent latestEvent; SceneObject* pointedObject; osg::Vec3d orgPointedPos; void initializeOsgObjects(); void initializeGL(); void setupMenu(MenuManager& mm); ToolBar* setupToolBar(); void onEditModeToggled(bool on); void setOperationMode(SceneObject::SceneMode mode); void flipOperationMode(); void initializeCamera(); void resetCameraProjection(); void correctCameraProjectionAspectRatio(); void viewAll(); osg::Node* createLights(); void setupHUD(); void setupCoordinateAxes(); osg::Node* createAxes(float length, float width); void enableCoordinateAxes(bool on, bool doRequestRedraw); void updateAxesPosition(); void setupFloorGrid(); void updateFloorGridLines(); void addSceneObject(SceneObject* object); void removeSceneObject(SceneObject* object); void onSceneObjectRedrawRequest(int flags); void setHomeView(); bool onViewerFocusOut(QFocusEvent* event); bool handleOsgEvent(const osgGA::GUIEventAdapter& event, osgGA::GUIActionAdapter& action); void handleOsgResizeEvent(const osgGA::GUIEventAdapter& event); void updateLatestEvent(QKeyEvent* event); void updateLatestEvent(int x, int y, int modifiers); void updateLatestEvent(QMouseEvent* event); virtual bool eventFilter(QObject* obj, QEvent* event); bool onKeyPressEvent(QKeyEvent* event); bool onKeyReleaseEvent(QKeyEvent* event); bool onMousePressEvent(QMouseEvent* event); bool onMouseReleaseEvent(QMouseEvent* event); bool onMouseDoubleClickEvent(QMouseEvent* event); bool onMouseMoveEvent(QMouseEvent* event); void updatePointerPosition(); bool onWheelEvent(QWheelEvent* event); SceneObject* findPointedObject(); void showViewModePopupMenu(const QPoint& globalPos); void showEditModePopupMenu(SceneObject* object, const QPoint& globalPos); void onPopupMenuSelectionDone(); void startViewRotation(); void dragViewRotation(QMouseEvent* event); void startViewTranslation(); void dragViewTranslation(); void startViewZoom(); void dragViewZoom(); void zoomView(double ratio); void onWireframeModeToggled(bool on); void onScribeModeToggled(bool on); void onCartoonModeToggled(bool on); void onFloorGridToggled(bool on); void onPerspectiveViewToggled(bool on); void onShadowToggled(bool on); void onCollisionToggled(bool on); void onOverPaintToggled(bool on); void onHiPriorityRenderingToggled(bool on); void onCaptureImageButtonClicked(); bool saveImage(const std::string& filename); void setScreenSize(int width, int height); bool storeState(Archive& archive); bool restoreState(const Archive& archive); }; } namespace { class SceneViewEventHandler : public osgGA::GUIEventHandler { public: SceneViewEventHandler(SceneViewImpl* sceneWindow) : sceneWindow(sceneWindow) { } bool handle(const osgGA::GUIEventAdapter& event, osgGA::GUIActionAdapter& action) { return sceneWindow->handleOsgEvent(event, action); } SceneViewImpl* sceneWindow; }; class OsgViewerEx : public OsgViewer { public: OsgViewerEx(SceneViewImpl* viewImpl) : OsgViewer(viewImpl->self), viewImpl(viewImpl) { } virtual void initializeGL() { viewImpl->initializeGL(); } SceneViewImpl* viewImpl; }; } SetupDialog::SetupDialog(SceneViewImpl* impl) : QDialog(MainWindow::instance()) { setWindowTitle(_("SceneView Setup")); QVBoxLayout* vbox = new QVBoxLayout(); vbox->addLayout(new HSeparatorBox(new QLabel(_("Floor Grid")))); QHBoxLayout* hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(_("Span"))); floorGridSpanSpin.setAlignment(Qt::AlignCenter); floorGridSpanSpin.setDecimals(1); floorGridSpanSpin.setRange(0.0, 99.9); floorGridSpanSpin.setSingleStep(0.1); floorGridSpanSpin.setValue(10.0); floorGridSpanSpin.sigValueChanged().connect(bind(&SceneViewImpl::updateFloorGridLines, impl)); hbox->addWidget(&floorGridSpanSpin); hbox->addSpacing(8); hbox->addWidget(new QLabel(_("Interval"))); floorGridIntervalSpin.setAlignment(Qt::AlignCenter); floorGridIntervalSpin.setDecimals(2); floorGridIntervalSpin.setRange(0.01, 9.99); floorGridIntervalSpin.setSingleStep(0.01); floorGridIntervalSpin.setValue(0.5); floorGridIntervalSpin.sigValueChanged().connect(bind(&SceneViewImpl::updateFloorGridLines, impl)); hbox->addWidget(&floorGridIntervalSpin); hbox->addStretch(); vbox->addLayout(hbox); vbox->addWidget(new HSeparator()); hbox = new QHBoxLayout(); hiPriorityRenderingCheck.setText(_("Hi-priority rendering mode")); hiPriorityRenderingCheck.setChecked(false); hiPriorityRenderingCheck.sigToggled().connect(bind(&SceneViewImpl::onHiPriorityRenderingToggled, impl, _1)); hbox->addWidget(&hiPriorityRenderingCheck); hbox->addStretch(); vbox->addLayout(hbox); vbox->addWidget(new HSeparator()); QPushButton* okButton = new QPushButton(_("&Ok")); okButton->setDefault(true); QDialogButtonBox* buttonBox = new QDialogButtonBox(this); buttonBox->addButton(okButton, QDialogButtonBox::AcceptRole); connect(buttonBox,SIGNAL(accepted()), this, SLOT(accept())); vbox->addWidget(buttonBox); setLayout(vbox); } void SetupDialog::storeState(Archive& archive) { archive.write("floorGridSpan", floorGridSpanSpin.value()); archive.write("floorGridInterval", floorGridIntervalSpin.value()); archive.write("hiPriorityRendering", hiPriorityRenderingCheck.isChecked()); } void SetupDialog::restoreState(const Archive& archive) { floorGridSpanSpin.setValue(archive.get("floorGridSpan", floorGridSpanSpin.value())); floorGridIntervalSpin.setValue(archive.get("floorGridInterval", floorGridIntervalSpin.value())); hiPriorityRenderingCheck.setChecked(archive.get("hiPriorityRendering", hiPriorityRenderingCheck.isChecked())); } SceneView* SceneView::mainInstance() { assert(mainSceneView); return mainSceneView; } void SceneView::initialize(ExtensionManager* ext) { if(!mainSceneView){ mainSceneView = new SceneView(); mainSceneView->impl->setupMenu(ext->menuManager()); ext->addView(mainSceneView); ext->addToolBar(mainSceneView->impl->setupToolBar()); } } SceneView::SceneView() { impl = new SceneViewImpl(this); } SceneViewImpl::SceneViewImpl(SceneView* self) : self(self), setup(this), os(MessageView::mainInstance()->cout()) { self->setName(N_("Scene")); self->setDefaultLayoutArea(View::RIGHT); self->setMinimumSize(1, 1); operationMode = SceneObject::VIEW_MODE; dragMode = NO_DRAGGING; pointedObject = 0; drawingAreaHeight = 0; latestEvent.sceneView = self; indicatorLabel = new QLabel(); indicatorLabel->setAlignment(Qt::AlignLeft); QFont font = indicatorLabel->font(); font.setFixedPitch(true); indicatorLabel->setFont(font); viewer = new OsgViewerEx(this); QHBoxLayout* layout = new QHBoxLayout(); layout->addWidget(viewer); self->setLayout(layout); viewer->addEventHandler(new SceneViewEventHandler(this)); viewer->getEventQueue()->getCurrentEventState() ->setMouseYOrientation(osgGA::GUIEventAdapter::Y_INCREASING_UPWARDS); viewer->setMouseTracking(true); viewer->installEventFilter(this); popupMenu = new Menu(self); menuManager.setTopMenu(popupMenu); popupMenu->sigTriggered().connect(bind(&SceneViewImpl::onPopupMenuSelectionDone, this)); viewer->requestRedraw(); } SceneView::~SceneView() { if(impl){ delete impl; } } SceneViewImpl::~SceneViewImpl() { if(TRACE_FUNCTIONS){ cout << "SceneViewImpl::~SceneViewImpl()" << endl; } delete indicatorLabel; } void SceneViewImpl::initializeGL() { if(TRACE_FUNCTIONS){ cout << "SceneViewImpl::initializeGL()" << endl; } static bool isFirst = true; if(isFirst){ initializeOsgObjects(); isFirst = false; } } void SceneViewImpl::initializeOsgObjects() { if(TRACE_FUNCTIONS){ cout << "SceneViewImpl::initializeOsgObjects()" << endl; } initializeCamera(); rootNode = new osg::Group; preprocessingNodeGroup = new osg::Group; rootNode->addChild(preprocessingNodeGroup.get()); rootNode->addChild(createLights()); objectGroupNode = new osg::Group; rootNode->addChild(objectGroupNode.get()); osg::StateSet* stateSet = objectGroupNode->getOrCreateStateSet(); polygonMode = new osg::PolygonMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::FILL); stateSet->setAttributeAndModes(polygonMode.get()); switchNode = new osg::Switch; rootNode->addChild(switchNode.get()); setupHUD(); rootNode->addChild(hudCamera.get()); setupCoordinateAxes(); setupFloorGrid(); viewer->setSceneData(rootNode.get()); } void SceneViewImpl::setupMenu(MenuManager& mm) { mm.setPath("/Options").setPath(N_("Scene View")); Action* coordinateAxesCheck = mm.addCheckItem(_("Coordinate axes")); coordinateAxesCheck->setChecked(true); coordinateAxesCheck->sigToggled().connect(bind(&SceneViewImpl::enableCoordinateAxes, this, _1, true)); Action* scribeCheck = mm.addCheckItem(_("Scribe mode")); scribeCheck->sigToggled().connect(bind(&SceneViewImpl::onScribeModeToggled, this, _1)); Action* cartoonCheck = mm.addCheckItem(_("Cartoon mode on/off")); cartoonCheck->sigToggled().connect(bind(&SceneViewImpl::onCartoonModeToggled, this, _1)); shadowCheck = mm.addCheckItem(_("toggle shadow on/off")); shadowCheck->sigToggled().connect(bind(&SceneViewImpl::onShadowToggled, this, _1)); Action* overPaintCheck = mm.addCheckItem(_("Over paint mode")); overPaintCheck->sigToggled().connect(bind(&SceneViewImpl::onOverPaintToggled, this, _1)); } ToolBar* SceneViewImpl::setupToolBar() { ToolBar* toolBar = new ToolBar(N_("SceneBar")); editModeToggle = toolBar->addToggleButton(QIcon(":/Base/icons/sceneedit.png"), _("Switch to the edit mode")); editModeToggle->setChecked(false); editModeToggle->sigToggled().connect(bind(&SceneViewImpl::onEditModeToggled, this, _1)); toolBar->addButton(QIcon(":/Base/icons/viewfitting.png"), _("Move the camera to look at the objects")) ->sigClicked().connect(bind(&SceneViewImpl::viewAll, this)); perspectiveToggle = toolBar->addToggleButton(QIcon(":/Base/icons/perspective.png"), _("Toggle the perspective / orthogonal view mode")); perspectiveToggle->setChecked(true); perspectiveToggle->sigToggled().connect(bind(&SceneViewImpl::onPerspectiveViewToggled, this, _1)); floorGridToggle = toolBar->addToggleButton(QIcon(":/Base/icons/floorgrid.png"), _("Show the floor grid")); floorGridToggle->setChecked(true); floorGridToggle->sigToggled().connect(bind(&SceneViewImpl::onFloorGridToggled, this, _1)); collisionToggle = toolBar->addToggleButton(QIcon(":/Base/icons/collisionlines.png"), _("Toggle the collision visualization")); collisionToggle->setChecked(true); viewer->isCollisionVisibleMode_ = collisionToggle->isChecked(); collisionToggle->sigToggled().connect(bind(&SceneViewImpl::onCollisionToggled, this, _1)); ToolButton* wireframeToggle = toolBar->addToggleButton(QIcon(":/Base/icons/wireframe.png"), _("Toggle the wireframe mode")); wireframeToggle->sigToggled().connect(bind(&SceneViewImpl::onWireframeModeToggled, this, _1)); toolBar->addButton(QIcon(":/Base/icons/scenecapture.png"), _("Capture a screen image and save it into a file")) ->sigClicked().connect(bind(&SceneViewImpl::onCaptureImageButtonClicked, this)); toolBar->addButton(QIcon(":/Base/icons/setup.png"), _("Open the dialog to setup SceneView")) ->sigClicked().connect(bind(&SetupDialog::show, &setup)); return toolBar; } QWidget* SceneView::indicatorOnInfoBar() { return impl->indicatorLabel; } void SceneViewImpl::onEditModeToggled(bool on) { setOperationMode(on ? SceneObject::EDIT_MODE : SceneObject::VIEW_MODE); } void SceneViewImpl::setOperationMode(SceneObject::SceneMode mode) { operationMode = mode; int n = objectGroupNode->getNumChildren(); for(int i=0; i < n; ++i){ SceneObject* object = dynamic_cast(objectGroupNode->getChild(i)); if(object){ object->sceneMode_ = mode; object->onSceneModeChanged(); } } } void SceneViewImpl::flipOperationMode() { editModeToggle->setChecked(operationMode == SceneObject::VIEW_MODE); } void SceneViewImpl::initializeCamera() { projectionMode = PERSPECTIVE; orgOrthoProjectionMatrix.makeOrtho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0); orgPerspectiveProjectionMatrix.makePerspective(40.0, 1.0, 1.0, 100.0); numOverriddenCameraControls = 0; camera = viewer->getCamera(); latestEvent.camera_ = camera; //camera->setComputeNearFarMode(osg::CullSettings::DO_NOT_COMPUTE_NEAR_FAR); camera->setClearColor(osg::Vec4(0.1, 0.1, 0.3, 1.0)); homeViewMatrix.makeLookAt(osg::Vec3d(4.0, 2.0, 1.5), osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 0.0, 1.0)); camera->setViewMatrix(homeViewMatrix); resetCameraProjection(); } void SceneViewImpl::resetCameraProjection() { if(numOverriddenCameraControls > 0){ return; } if(projectionMode == PERSPECTIVE){ camera->setProjectionMatrix(orgPerspectiveProjectionMatrix); } else { camera->setProjectionMatrix(orgOrthoProjectionMatrix); } correctCameraProjectionAspectRatio(); } void SceneViewImpl::correctCameraProjectionAspectRatio() { double w = viewer->width(); double h = viewer->height(); if(projectionMode == PERSPECTIVE){ double fovy, aspect, zNear, zFar; camera->getProjectionMatrixAsPerspective(fovy, aspect, zNear, zFar); camera->setProjectionMatrixAsPerspective(fovy, (w / h), zNear, zFar); } else { double left, right, bottom, top, zNear, zFar; camera->getProjectionMatrixAsOrtho(left, right, bottom, top, zNear, zFar); double hw = (right - left) / 2.0; double hh = (h / w) * hw; camera->setProjectionMatrixAsOrtho(-hw, hw, -hh, hh, zNear, zFar); } } void SceneViewImpl::viewAll() { osg::BoundingSphere bsphere = objectGroupNode->getBound(); if(bsphere.radius() <= 0.0){ bsphere.set(osg::Vec3d(0.0, 0.0, 0.0), 3.0); } osg::Vec3d eye, center, up; camera->getViewMatrixAsLookAt(eye, center, up); osg::Matrixd T = camera->getViewMatrix(); osg::Vec3d p = osg::Matrixd::transform3x3(bsphere.center() - eye, T); double left, right, bottom, top, zNear, zFar; if(projectionMode == PERSPECTIVE){ camera->getProjectionMatrixAsFrustum(left, right, bottom, top, zNear, zFar); p[2] += 2.0 * bsphere.radius() * zNear / (right - left); } else { camera->getProjectionMatrixAsOrtho(left, right, bottom, top, zNear, zFar); double z = bsphere.center()[2]; double r = bsphere.radius(); double s = r * 2.0 / (right - left); camera->setProjectionMatrixAsOrtho(left * s, right * s, bottom * s, top * s, z + r, z - r); p[2] = 0.0; } osg::Vec3d p2 = osg::Matrixd::transform3x3(T, p); T.preMultTranslate(-p2); camera->setViewMatrix(T); viewer->requestRedraw(); } void SceneViewImpl::setupHUD() { hudCamera = new osg::Camera; hudCamera->setReferenceFrame(osg::Transform::ABSOLUTE_RF); hudCamera->setViewMatrix(osg::Matrix::identity()); hudCamera->setComputeNearFarMode(osg::CullSettings::DO_NOT_COMPUTE_NEAR_FAR); hudCamera->setClearMask(GL_DEPTH_BUFFER_BIT); hudCamera->setRenderOrder(osg::Camera::POST_RENDER); hudCamera->setAllowEventFocus(false); osg::StateSet* stateset = hudCamera->getOrCreateStateSet(); stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); // sample of putting a text if(false){ osg::Geode* geode = new osg::Geode(); osgText::Text* text = new osgText::Text; osgText::Font* font = osgText::readFontFile("fonts/arial.ttf"); //font->setMagFilterHint(osg::Texture::NEAREST); text->setFont(font); //text->setBackdropType(osgText::Text::NONE); //text->setCharacterSizeMode(osgText::TextBase::OBJECT_COORDS); //text->setCharacterSize(16); text->setPosition(osg::Vec3(60.0f, 20.0f, 0.0f)); text->setText("Head Up Displays are simple :-)"); text->update(); geode->addDrawable(text); if(false){ osg::BoundingBox bb; for(unsigned int i=0;igetNumDrawables();++i){ bb.expandBy(geode->getDrawable(i)->getBound()); } osg::Geometry* geom = new osg::Geometry; osg::Vec3Array* vertices = new osg::Vec3Array; float depth = bb.zMin()-0.1; vertices->push_back(osg::Vec3(bb.xMin(),bb.yMax(),depth)); vertices->push_back(osg::Vec3(bb.xMin(),bb.yMin(),depth)); vertices->push_back(osg::Vec3(bb.xMax(),bb.yMin(),depth)); vertices->push_back(osg::Vec3(bb.xMax(),bb.yMax(),depth)); geom->setVertexArray(vertices); osg::Vec3Array* normals = new osg::Vec3Array; normals->push_back(osg::Vec3(0.0f,0.0f,1.0f)); geom->setNormalArray(normals); geom->setNormalBinding(osg::Geometry::BIND_OVERALL); osg::Vec4Array* colors = new osg::Vec4Array; colors->push_back(osg::Vec4(1.0f,1.0,0.8f,0.2f)); geom->setColorArray(colors); geom->setColorBinding(osg::Geometry::BIND_OVERALL); geom->addPrimitiveSet(new osg::DrawArrays(GL_QUADS,0,4)); osg::StateSet* stateset = geom->getOrCreateStateSet(); stateset->setMode(GL_BLEND,osg::StateAttribute::ON); //stateset->setAttribute(new osg::PolygonOffset(1.0f,1.0f),osg::StateAttribute::ON); stateset->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); geode->addDrawable(geom); } hudCamera->addChild(geode); } } osg::Node* SceneViewImpl::createLights() { osg::Light* light = viewer->getLight(); light->setDiffuse(osg::Vec4(0.7f,0.7f,0.7f,1.0f)); light->setSpecular(osg::Vec4(0.7f,0.7f,0.7f,1.0f)); // lights osg::Group* lightGroup (new osg::Group); osg::ref_ptr lightSS (rootNode->getOrCreateStateSet()); osg::ref_ptr lightSource1 = new osg::LightSource; osg::ref_ptr lightSource2 = new osg::LightSource; // create a local light. osg::Vec4f lightPosition (osg::Vec4f(0.0, 0.0, 10.0, 1.0f)); // point light //osg::Vec4f lightPosition(osg::Vec4f(0.0, -1.0, 0.0, 0.0f)); // directional light //osg::Vec4f lightPosition(osg::Vec4f(1.0, 0.0, 0.0, 0.0f)); // directional light osg::ref_ptr myLight = new osg::Light; myLight->setLightNum(1); myLight->setPosition(lightPosition); //myLight->setAmbient(osg::Vec4(0.2f,0.2f,0.2f,1.0f)); myLight->setAmbient(osg::Vec4(0.0f,0.0f,0.0f,0.0f)); myLight->setDiffuse(osg::Vec4(0.7f,0.7f,0.7f,1.0f)); //myLight->setDiffuse(osg::Vec4(0.6f,0.6f,0.6f,1.0f)); myLight->setSpecular(osg::Vec4(0.7f,0.7f,0.7f,1.0f)); //myLight->setSpecular(osg::Vec4(0.4f,0.4f,0.4f,1.0f)); //myLight->setSpecular(osg::Vec4(0.0f,0.0f,0.0f,0.0f)); myLight->setConstantAttenuation(1.0f); lightSource1->setLight(myLight.get()); lightSource1->setLocalStateSetModes(osg::StateAttribute::ON); lightSource1->setStateSetModes(*lightSS,osg::StateAttribute::ON); //osg::StateSet* lightSS (lightGroup->getOrCreateStateSet()); // create a local light. osg::Vec4f lightPosition2 (osg::Vec4f(2.0,-1.0,3.0,1.0f)); osg::ref_ptr myLight2 = new osg::Light; myLight2->setLightNum(2); myLight2->setPosition(lightPosition2); myLight2->setAmbient(osg::Vec4(0.2f,0.2f,0.2f,1.0f)); myLight2->setDiffuse(osg::Vec4(0.4f,0.1f,0.1f,1.0f)); myLight2->setConstantAttenuation(1.0f); lightSource2->setLight(myLight2.get()); lightSource2->setLocalStateSetModes(osg::StateAttribute::ON); lightSource2->setStateSetModes(*lightSS,osg::StateAttribute::ON); lightGroup->addChild(lightSource1.get()); //lightGroup->addChild(lightSource2.get()); return lightGroup; } void SceneViewImpl::setupCoordinateAxes() { coordinateAxesEnabled = false; axesTransform = new osg::PositionAttitudeTransform(); if(ENABLE_PERSPECTIVE_COORDINATE_AXES){ axesTransform->addChild(createAxes(0.1, 0.01)); } else { axesTransform->setPosition(osg::Vec3d(26.0, 24.0, 0.0)); axesTransform->addChild(createAxes(16.0, 2.0)); } enableCoordinateAxes(true, false); } osg::Node* SceneViewImpl::createAxes(float length, float width) { osg::Cone* cones[3]; osg::Cylinder* cylinders[3]; for(int i=0; i < 3; ++i){ cones[i] = new osg::Cone(); cones[i]->setRadius(width * 2.0); cones[i]->setHeight(width * 2.0 * 2.0); cylinders[i] = new osg::Cylinder(); cylinders[i]->setRadius(width); cylinders[i]->setHeight(length); } // x cones[0]->setRotation(osg::Quat(1.571f, osg::Vec3(0.0f, 1.0f, 0.0f))); cones[0]->setCenter(osg::Vec3(length, 0.0f, 0.0f)); cylinders[0]->setRotation(osg::Quat(1.571f, osg::Vec3(0.0f, 1.0f, 0.0f))); cylinders[0]->setCenter(osg::Vec3(length / 2.0f, 0.0f, 0.0f)); // y cones[1]->setRotation(osg::Quat(1.571f, osg::Vec3(-1.0f, 0.0f, 0.0f))); cones[1]->setCenter(osg::Vec3(0.0f, length, 0.0f)); cylinders[1]->setRotation(osg::Quat(1.571f, osg::Vec3(-1.0f, 0.0f, 0.0f))); cylinders[1]->setCenter(osg::Vec3(0.0f, length / 2.0f, 0.0f)); // z cones[2]->setCenter(osg::Vec3(0.0f, 0.0f, length)); cylinders[2]->setCenter(osg::Vec3(0.0f, 0.0f, length / 2.0f)); osg::Geode* geode = new osg::Geode; float c[][3] = { { 1.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, { 0.4f, 0.6f, 1.0f } }; for(int i=0; i < 3; ++i){ osg::Vec4 color(c[i][0], c[i][1], c[i][2], 1.0f); osg::ShapeDrawable* dCone = new osg::ShapeDrawable(cones[i]); dCone->setColor(color); geode->addDrawable(dCone); osg::ShapeDrawable* dCylinder = new osg::ShapeDrawable(cylinders[i]); dCylinder->setColor(color); geode->addDrawable(dCylinder); } geode->setDataVariance(osg::Object::STATIC); return geode; } void SceneViewImpl::enableCoordinateAxes(bool on, bool doRequestRedraw) { if(coordinateAxesEnabled != on){ if(on){ if(ENABLE_PERSPECTIVE_COORDINATE_AXES){ rootNode->addChild(axesTransform.get()); } else { hudCamera->addChild(axesTransform.get()); } updateAxesPosition(); } else { if(ENABLE_PERSPECTIVE_COORDINATE_AXES){ rootNode->removeChild(axesTransform.get()); } else { hudCamera->removeChild(axesTransform.get()); } } coordinateAxesEnabled = on; if(doRequestRedraw){ viewer->requestRedraw(); } } } void SceneViewImpl::updateAxesPosition() { if(coordinateAxesEnabled){ if(ENABLE_PERSPECTIVE_COORDINATE_AXES){ osg::Vec3d p(0.3, -0.3, -1.5); axesTransform->setPosition(p * camera->getInverseViewMatrix()); } else { axesTransform->setAttitude(camera->getViewMatrix().getRotate()); } } } void SceneViewImpl::setupFloorGrid() { floorGridNode = new osg::Geode; floorGridNode->setDataVariance(osg::Object::STATIC); osg::StateSet* state = floorGridNode->getOrCreateStateSet(); state->setMode(GL_LIGHTING, osg::StateAttribute::OFF); //state->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON); //state->setMode(GL_BLEND, osg::StateAttribute::ON); osg::Material* mat = new osg::Material; osg::Vec4 color(0.9f, 0.9f, 0.9f, 1.0f); mat->setDiffuse(osg::Material::FRONT_AND_BACK, color); state->setAttribute(mat); updateFloorGridLines(); switchNode->addChild(floorGridNode.get()); } void SceneViewImpl::updateFloorGridLines() { int prevNumDrawables = floorGridNode->getNumDrawables(); if(prevNumDrawables > 0){ floorGridNode->removeDrawables(0, prevNumDrawables); } osg::Geometry* geom = new osg::Geometry; osg::Vec3Array* v = new osg::Vec3Array; geom->setVertexArray(v); double half = setup.floorGridSpanSpin.value() / 2.0; double interval = setup.floorGridIntervalSpin.value(); double i = 0.0f; double x = 0.0f; do { x = i * interval; // y-line v->push_back(osg::Vec3( x, -half, 0.0f)); v->push_back(osg::Vec3( x, half, 0.0f)); v->push_back(osg::Vec3(-x, -half, 0.0f)); v->push_back(osg::Vec3(-x, half, 0.0f)); // x-line v->push_back(osg::Vec3(-half, x, 0.0f)); v->push_back(osg::Vec3( half, x, 0.0f)); v->push_back(osg::Vec3(-half, -x, 0.0f)); v->push_back(osg::Vec3( half, -x, 0.0f)); ++i; } while(x < half); geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, v->size())); floorGridNode->addDrawable(geom); if(prevNumDrawables > 0){ viewer->requestRedraw(); } } OsgViewer* SceneView::viewer() { return impl->viewer; } void SceneView::updateIndicator(const std::string& text) { impl->indicatorLabel->setText(text.c_str()); } /** @note The type of 'object' should not be a ref_ptr type but a plain pointer type because osg::ref_ptr does not seem to support the automatic conversion to the ref_ptrs of the super classes. */ void SceneView::addSceneObject(SceneObject* object) { impl->addSceneObject(object); } void SceneViewImpl::addSceneObject(SceneObject* object) { objectGroupNode->addChild(object); object->sigRedrawRequest.connect(bind(&SceneViewImpl::onSceneObjectRedrawRequest, this, _1)); if(object->getNumParents() == 1){ object->onAttachedToScene(); object->isActive_ = true; } if(object->sceneMode_ != operationMode){ object->sceneMode_ = operationMode; object->onSceneModeChanged(); } } /** @note The type of 'object' should not be a ref_ptr type but a plain pointer type because osg::ref_ptr does not seem to support the automatic conversion to the ref_ptrs of the super classes. */ void SceneView::removeSceneObject(SceneObject* object) { impl->removeSceneObject(object); } void SceneViewImpl::removeSceneObject(SceneObject* object) { if(object == pointedObject){ pointedObject = 0; } objectGroupNode->removeChild(object); object->sigRedrawRequest.disconnect(bind(&SceneViewImpl::onSceneObjectRedrawRequest, this, _1)); if(object->getNumParents() == 0){ object->onDetachedFromScene(); object->isActive_ = false; } } void SceneViewImpl::onSceneObjectRedrawRequest(int flags) { if(flags & SceneObject::EFFECT_NODE_CREATED){ //viewer->requestRedrawWhenEffectNodeCreated(); viewer->requestRedraw(); } else { viewer->requestRedraw(); } } void SceneView::requestRedraw() { impl->updateAxesPosition(); /// \todo move this ? impl->viewer->requestRedraw(); } void SceneViewImpl::setHomeView() { camera->setViewMatrix(homeViewMatrix); updateAxesPosition(); viewer->requestRedraw(); } bool SceneViewImpl::onViewerFocusOut(QFocusEvent* event) { return false; } bool SceneViewImpl::handleOsgEvent(const osgGA::GUIEventAdapter& event, osgGA::GUIActionAdapter& action) { bool handled = true; switch(event.getEventType()){ case osgGA::GUIEventAdapter::RESIZE: handleOsgResizeEvent(event); break; default: handled = false; break; } return handled; } void SceneViewImpl::handleOsgResizeEvent(const osgGA::GUIEventAdapter& event) { drawingAreaHeight = event.getWindowHeight(); // just the area of the axis hudCamera->setProjectionMatrixAsOrtho(0, 50, 0, 50, -20.0, 20.0); hudCamera->setViewport(0, 0, 50, 50); correctCameraProjectionAspectRatio(); } void SceneViewImpl::updateLatestEvent(QKeyEvent* event) { latestEvent.modKeyMask_ = OsgWidget::convertToOsgModKeyMask(event->modifiers()); latestEvent.key_ = OsgWidget::convertToOsgKeySymbol(event->key()); } void SceneViewImpl::updateLatestEvent(int x, int y, int modifiers) { latestEvent.x_ = x; latestEvent.y_ = drawingAreaHeight - y; latestEvent.modKeyMask_ = OsgWidget::convertToOsgModKeyMask(modifiers); } void SceneViewImpl::updateLatestEvent(QMouseEvent* event) { updateLatestEvent(event->x(), event->y(), event->modifiers()); latestEvent.button_ = OsgWidget::convertToOsgMouseButtonMask(event->button()); } bool SceneViewImpl::eventFilter(QObject* obj, QEvent* event) { if(obj == viewer){ switch(event->type()){ case QEvent::KeyPress: return onKeyPressEvent(static_cast(event)); case QEvent::KeyRelease: return onKeyReleaseEvent(static_cast(event)); case QEvent::MouseButtonPress: return onMousePressEvent(static_cast(event)); case QEvent::MouseButtonDblClick: return onMouseDoubleClickEvent(static_cast(event)); case QEvent::MouseButtonRelease: return onMouseReleaseEvent(static_cast(event)); case QEvent::MouseMove: return onMouseMoveEvent(static_cast(event)); case QEvent::Wheel: return onWheelEvent(static_cast(event)); case QEvent::FocusOut: return onViewerFocusOut(static_cast(event)); default: return false; } } return false; } bool SceneViewImpl::onKeyPressEvent(QKeyEvent* event) { bool handled = false; updateLatestEvent(event); switch(event->key()){ case Qt::Key_Escape: flipOperationMode(); handled = true; break; case Qt::Key_Z: if(event->modifiers() & Qt::ControlModifier){ if(pointedObject){ if(event->modifiers() & Qt::ShiftModifier){ pointedObject->onRedoRequest(); } else { pointedObject->onUndoRequest(); } } handled = true; } break; default: break; } if(!handled && operationMode == SceneObject::EDIT_MODE){ if(pointedObject){ if(pointedObject->onKeyPressEvent(latestEvent)){ handled = true; } } } return handled; } bool SceneViewImpl::onKeyReleaseEvent(QKeyEvent* event) { updateLatestEvent(event); bool handled = false; if(dragMode == VIEW_ZOOM && (event->key() == Qt::Key_Control)){ dragMode = NO_DRAGGING; } if(operationMode == SceneObject::EDIT_MODE){ if(pointedObject){ handled = pointedObject->onKeyReleaseEvent(latestEvent); } } return handled; } bool SceneViewImpl::onMousePressEvent(QMouseEvent* event) { updateLatestEvent(event); SceneObject* object = findPointedObject(); bool handled = false; SceneObject::SceneMode tmpOperationMode = operationMode; if(event->modifiers() & Qt::AltModifier){ tmpOperationMode = SceneObject::VIEW_MODE; } if(tmpOperationMode == SceneObject::EDIT_MODE){ if(pointedObject && pointedObject != object){ handled = pointedObject->onButtonPressEvent(latestEvent); } if(event->button() == Qt::RightButton){ showEditModePopupMenu(object, event->globalPos()); handled = true; } else if(object){ if(object->onButtonPressEvent(latestEvent)){ handled = true; pointedObject = object; dragMode = OBJECT_DELEGATION; } } } if(tmpOperationMode == SceneObject::VIEW_MODE || !handled){ switch(event->button()){ case Qt::LeftButton: startViewRotation(); break; case Qt::MidButton: if(event->modifiers() & Qt::ControlModifier){ startViewZoom(); } else { startViewTranslation(); } break; case Qt::RightButton: if(operationMode == SceneObject::VIEW_MODE){ showViewModePopupMenu(event->globalPos()); } break; default: break; } } return true; } bool SceneViewImpl::onMouseDoubleClickEvent(QMouseEvent* event) { updateLatestEvent(event); SceneObject* object = findPointedObject(); bool handled = false; if(operationMode == SceneObject::EDIT_MODE && object){ handled = object->onDoubleClickEvent(latestEvent); } if(!handled){ flipOperationMode(); } return true; } bool SceneViewImpl::onMouseReleaseEvent(QMouseEvent* event) { updateLatestEvent(event); if(operationMode == SceneObject::EDIT_MODE){ if(pointedObject){ pointedObject->onButtonReleaseEvent(latestEvent); } } dragMode = NO_DRAGGING; return true; } bool SceneViewImpl::onMouseMoveEvent(QMouseEvent* event) { if(TRACE_FUNCTIONS){ os << "SceneViewImpl::onMouseMoveEvent()" << endl; } updateLatestEvent(event->x(), event->y(), event->modifiers()); switch(dragMode){ case OBJECT_DELEGATION: pointedObject->onPointerMoveEvent(latestEvent); break; case VIEW_ROTATION: dragViewRotation(event); break; case VIEW_TRANSLATION: dragViewTranslation(); break; case VIEW_ZOOM: dragViewZoom(); break; default: updatePointerPosition(); break; } return true; } void SceneViewImpl::updatePointerPosition() { if(TRACE_FUNCTIONS){ os << "SceneViewImpl::updatePointerPosition()" << endl; } SceneObject* object = findPointedObject(); if(operationMode == SceneObject::EDIT_MODE){ if(pointedObject && (pointedObject != object)){ pointedObject->onPointerLeaveEvent(latestEvent); //pointedObject = 0; } if(object){ if(object->onPointerMoveEvent(latestEvent)){ pointedObject = object; if(!viewer->hasFocus()){ viewer->setFocus(Qt::MouseFocusReason); } } } } else { static boost::format f(_("Position = (%.3f %.3f %.3f)")); const osg::Vec3d& p = latestEvent.point(); self->updateIndicator(str(f % p.x() % p.y() % p.z())); } } bool SceneViewImpl::onWheelEvent(QWheelEvent* event) { updateLatestEvent(event->x(), event->y(), event->modifiers()); latestEvent.scrollingMotion_ = OsgWidget::convertToOsgScrollingMotion(event); latestEvent.scrollingDelta_ = static_cast(event->delta()); bool handled = false; if(operationMode == SceneObject::EDIT_MODE){ SceneObject* object = findPointedObject(); if(object && object->onScrollEvent(latestEvent)){ pointedObject = object; handled = true; } } if(operationMode == SceneObject::VIEW_MODE || !handled){ double r = (event->modifiers() & Qt::ShiftModifier) ? 0.05 : 0.25; switch(latestEvent.scrollingMotion()){ case osgGA::GUIEventAdapter::SCROLL_UP: zoomView(1.0 + r); handled = true; break; case osgGA::GUIEventAdapter::SCROLL_DOWN: zoomView(1.0 - r); handled = true; break; default: break; } } return handled; } SceneObject* SceneViewImpl::findPointedObject() { if(TRACE_FUNCTIONS){ os << "SceneViewImpl::findPointedObject(): x, y = " << latestEvent.x() << ", " << latestEvent.y() << endl; } SceneObject* object = 0; latestEvent.path_.clear(); osgUtil::LineSegmentIntersector::Intersections intersections; if(viewer->computeIntersections(latestEvent.x(), latestEvent.y(), intersections)){ const osgUtil::LineSegmentIntersector::Intersection& intersection = *intersections.begin(); if(!intersection.nodePath.empty()){ bool isPointingIndicator = false; const osg::NodePath& path = intersection.nodePath; for(size_t i=0; i < path.size(); ++i){ osg::Node* node = path[i]; if(node == axesTransform){ isPointingIndicator = true; break; } object = dynamic_cast(node); if(object){ if(object->isEditable()){ latestEvent.path_.resize(path.size() - i); std::copy(path.begin() + i, path.end(), latestEvent.path_.begin()); } else { object = 0; } break; } } if(!isPointingIndicator){ latestEvent.point_ = intersection.getWorldIntersectPoint(); latestEvent.normal_ = intersection.getWorldIntersectNormal(); if(!object){ latestEvent.path_ = path; } } } } return object; } void SceneViewImpl::showViewModePopupMenu(const QPoint& globalPos) { menuManager.releaseAllItems(); menuManager.addItem(_("Edit Mode")) ->sigTriggered().connect(bind(&SceneViewImpl::flipOperationMode, this)); popupMenu->popup(globalPos); } void SceneViewImpl::showEditModePopupMenu(SceneObject* object, const QPoint& globalPos) { menuManager.releaseAllItems(); menuManager.setBackwardMode().addItem(_("View Mode")) ->sigTriggered().connect(bind(&SceneViewImpl::flipOperationMode, this)); if(object){ menuManager.addSeparator(); menuManager.setPath("/"); object->onContextMenuRequest(latestEvent, menuManager); } popupMenu->popup(globalPos); } void SceneViewImpl::onPopupMenuSelectionDone() { menuManager.releaseAllItems(); } void SceneViewImpl::startViewRotation() { if(numOverriddenCameraControls > 0){ return; } orgMouseX = latestEvent.x(); orgMouseY = latestEvent.y(); orgPointedPos = latestEvent.point(); dragMode = VIEW_ROTATION; orgViewMatrix = camera->getViewMatrix(); osg::Matrixd inv; inv.invert(orgViewMatrix); orgEye = inv.getTrans(); orgViewUp = osg::Matrixd::transform3x3(orgViewMatrix, osg::Vec3(0.0f, 1.0f, 0.0f)); orgViewSide = osg::Matrixd::transform3x3(orgViewMatrix, osg::Vec3(1.0f, 0.0f, 0.0f)); } void SceneViewImpl::dragViewRotation(QMouseEvent* event) { static const float angleRatio = 0.01; double dx = latestEvent.x() - orgMouseX; double dy = latestEvent.y() - orgMouseY; osg::Matrixd T; T.makeTranslate(orgPointedPos); osg::Matrixd invT; invT.invert(T); osg::Matrixd C(orgViewMatrix); osg::Matrixd invRu; //invRu.makeRotate(dx * angleRatio, orgViewUp); invRu.makeRotate(dx * angleRatio, osg::Vec3d(0.0, 0.0, 1.0)); osg::Matrixd invRs; invRs.makeRotate(-dy * angleRatio, orgViewSide); T.preMult(invRs); T.preMult(invRu); T.preMult(invT); osg::Camera* camera = viewer->getCamera(); camera->setViewMatrix(T * orgViewMatrix); // snap rotation if(event->modifiers() & Qt::ShiftModifier){ osg::Matrixd& T = camera->getViewMatrix(); Matrix3 R; R << T(0,0), T(1,0), T(2,0), T(0,1), T(1,1), T(2,1), T(0,2), T(1,2), T(2,2); Vector3 rpy = rpyFromRot(R); for(int i=0; i < 3; ++i){ double& a = rpy[i]; for(int j=0; j < 5; ++j){ double b = j * (PI / 2.0) - PI; if(fabs(b - a) < (PI / 8)){ a = b; break; } } } R = rotFromRpy(rpy); for(int i=0; i < 3; ++i){ for(int j=0; j < 3; ++j){ T(i,j) = R(j,i); } } camera->setViewMatrix(T); } updateAxesPosition(); viewer->requestRedraw(); } void SceneViewImpl::startViewTranslation() { if(numOverriddenCameraControls > 0){ return; } osg::Viewport* viewport = camera->getViewport(); double r = 1.0; double left, right, bottom, top, zNear, zFar; if(projectionMode == PERSPECTIVE){ osg::Vec3d eye, center, up; camera->getViewMatrixAsLookAt(eye, center, up); camera->getProjectionMatrixAsFrustum(left, right, bottom, top, zNear, zFar); osg::Vec3d az = center - eye; az.normalize(); r = ((latestEvent.point() - eye) * az) / zNear; } else { camera->getProjectionMatrixAsOrtho(left, right, bottom, top, zNear, zFar); } viewTranslationRatioX = r * (right - left) / viewport->width(); viewTranslationRatioY = r * (top - bottom) / viewport->height(); orgMouseX = latestEvent.x(); orgMouseY = latestEvent.y(); dragMode = VIEW_TRANSLATION; orgViewMatrix = camera->getViewMatrix(); orgViewSide = osg::Matrixd::transform3x3(orgViewMatrix, osg::Vec3(1.0f, 0.0f, 0.0f)); orgViewUp = osg::Matrixd::transform3x3(orgViewMatrix, osg::Vec3(0.0f, 1.0f, 0.0f)); } void SceneViewImpl::dragViewTranslation() { osg::Camera* camera = viewer->getCamera(); double dx = viewTranslationRatioX * (latestEvent.x() - orgMouseX); double dy = viewTranslationRatioY * (latestEvent.y() - orgMouseY); osg::Matrixd invT; invT.makeTranslate(orgViewSide * dx + orgViewUp * dy); camera->setViewMatrix(invT * orgViewMatrix); updateAxesPosition(); viewer->requestRedraw(); } void SceneViewImpl::startViewZoom() { if(numOverriddenCameraControls > 0){ return; } dragMode = VIEW_ZOOM; orgMouseY = latestEvent.y(); if(projectionMode == PERSPECTIVE){ orgViewMatrix = camera->getViewMatrix(); orgViewForward = osg::Matrixd::transform3x3(orgViewMatrix, osg::Vec3(0.0f, 0.0f, 1.0f)); osg::Vec3d eye, center, up; camera->getViewMatrixAsLookAt(eye, center, up); orgZoomDistance = fabs((latestEvent.point() - eye) * orgViewForward); } else { orgOrthoProjectionMatrix = camera->getProjectionMatrix(); } } void SceneViewImpl::dragViewZoom() { double dy = latestEvent.y() - orgMouseY; if(projectionMode == PERSPECTIVE){ osg::Matrixd invT; invT.makeTranslate(orgViewForward * (orgZoomDistance * (-exp(dy * 0.01) + 1.0))); camera->setViewMatrix(invT * orgViewMatrix); } else { double p[4]; double zNear, zFar; orgOrthoProjectionMatrix.getOrtho(p[0], p[1], p[2], p[3], zNear, zFar); double ratio = exp(dy * 0.01); for(int i=0; i < 4; ++i){ p[i] *= ratio; } camera->setProjectionMatrixAsOrtho(p[0], p[1], p[2], p[3], zNear, zFar); } correctCameraProjectionAspectRatio(); updateAxesPosition(); viewer->requestRedraw(); } void SceneViewImpl::zoomView(double ratio) { if(numOverriddenCameraControls > 0){ return; } if(projectionMode == PERSPECTIVE){ orgViewMatrix = camera->getViewMatrix(); orgViewForward = osg::Matrixd::transform3x3(orgViewMatrix, osg::Vec3(0.0f, 0.0f, -1.0f)); osg::Vec3d eye, center, up; camera->getViewMatrixAsLookAt(eye, center, up); double dz = (ratio - 1.0) * ((latestEvent.point() - eye) * orgViewForward); osg::Matrixd invT; invT.makeTranslate(orgViewForward * dz); camera->setViewMatrix(invT * orgViewMatrix); } else { double p[4]; double zNear, zFar; camera->getProjectionMatrixAsOrtho(p[0], p[1], p[2], p[3], zNear, zFar); for(int i=0; i < 4; ++i){ p[i] *= ratio; } camera->setProjectionMatrixAsOrtho(p[0], p[1], p[2], p[3], zNear, zFar); } correctCameraProjectionAspectRatio(); updateAxesPosition(); viewer->requestRedraw(); } void SceneViewImpl::onWireframeModeToggled(bool on) { osg::PolygonMode::Mode mode = on ? osg::PolygonMode::LINE : osg::PolygonMode::FILL; polygonMode->setMode(osg::PolygonMode::FRONT_AND_BACK, mode); viewer->requestRedraw(); } void SceneViewImpl::onScribeModeToggled(bool on) { if(on){ if(!scribe){ scribe = new osgFX::Scribe(); scribe->setEnabled(true); } scribe->addChild(objectGroupNode.get()); rootNode->replaceChild(objectGroupNode.get(), scribe.get()); } else { if(scribe.valid()){ rootNode->replaceChild(scribe.get(), objectGroupNode.get()); } } //viewer->requestRedrawWhenEffectNodeCreated(); viewer->requestRedraw(); } void SceneViewImpl::onCartoonModeToggled(bool on) { if(on){ if(!cartoon){ cartoon = new osgFX::Cartoon(); //cartoon->setOutlineLineWidth(20.0f); } cartoon->addChild(objectGroupNode.get()); rootNode->replaceChild(objectGroupNode.get(), cartoon.get()); } else { if(cartoon.valid()){ rootNode->replaceChild(cartoon.get(), objectGroupNode.get()); } } //viewer->requestRedrawWhenEffectNodeCreated(); viewer->requestRedraw(); } void SceneView::toggleFloorGrid(bool on) { impl->floorGridToggle->setChecked(on); } void SceneViewImpl::onFloorGridToggled(bool on) { switchNode->setChildValue(floorGridNode.get(), on); viewer->requestRedraw(); } void SceneViewImpl::onPerspectiveViewToggled(bool on) { ProjectionMode newProjectionMode = on ? PERSPECTIVE : ORTHO; if(newProjectionMode != projectionMode){ if(projectionMode == PERSPECTIVE){ orgPerspectiveProjectionMatrix = camera->getProjectionMatrix(); } else { orgOrthoProjectionMatrix = camera->getProjectionMatrix(); } projectionMode = newProjectionMode; resetCameraProjection(); viewer->requestRedraw(); } } void SceneView::toggleShadow(bool on) { impl->shadowCheck->setChecked(on); } void SceneViewImpl::onShadowToggled(bool on) { const bool UseShadowMap = false; const int ReceivesShadowTraversalMask = 0x1; const int CastsShadowTraversalMask = 0x2; if(on){ if(!shadowedScene){ shadowedScene = new osgShadow::ShadowedScene(); shadowedScene->setReceivesShadowTraversalMask(ReceivesShadowTraversalMask); shadowedScene->setCastsShadowTraversalMask(CastsShadowTraversalMask); if(UseShadowMap){ shadowMap = new osgShadow::ShadowMap(); shadowedScene->setShadowTechnique(shadowMap.get()); int mapres = 1024; shadowMap->setTextureSize(osg::Vec2s(mapres, mapres)); } else { osgShadow::ShadowTexture* st = new osgShadow::ShadowTexture(); shadowedScene->setShadowTechnique(st); } } shadowedScene->addChild(objectGroupNode.get()); rootNode->replaceChild(objectGroupNode.get(), shadowedScene.get()); if(!UseShadowMap){ objectGroupNode->getChild(0)->setNodeMask(CastsShadowTraversalMask); objectGroupNode->getChild(1)->setNodeMask(ReceivesShadowTraversalMask); } else { osg::ref_ptr matirial = new osg::Material; matirial->setColorMode(osg::Material::DIFFUSE); matirial->setAmbient(osg::Material::FRONT_AND_BACK, osg::Vec4(0, 0, 0, 1)); matirial->setSpecular(osg::Material::FRONT_AND_BACK, osg::Vec4(1, 1, 1, 1)); matirial->setShininess(osg::Material::FRONT_AND_BACK, 64); objectGroupNode->getOrCreateStateSet()->setAttributeAndModes(matirial.get(), osg::StateAttribute::ON); } //objectGroupNode->setNodeMask(ReceivesShadowTraversalMask); } else { if(shadowedScene.valid()){ rootNode->replaceChild(shadowedScene.get(), objectGroupNode.get()); } } viewer->frame(); viewer->requestRedraw(); } void SceneViewImpl::onCollisionToggled(bool on) { viewer->isCollisionVisibleMode_ = on; viewer->requestRedraw(); } void SceneViewImpl::onOverPaintToggled(bool on) { if(on){ camera->setClearMask(GL_DEPTH_BUFFER_BIT); } else { camera->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); } viewer->requestRedraw(); } osg::Camera* SceneView::getCameraControl() { impl->numOverriddenCameraControls++; return impl->camera.get(); } bool SceneView::releaseCameraControl(osg::Camera* camera) { impl->numOverriddenCameraControls--; if(impl->numOverriddenCameraControls >= 0){ return true; } else { impl->numOverriddenCameraControls = 0; return false; } } void SceneView::addPreprocessingNode(osg::Node* node) { if(!impl->preprocessingNodeGroup->containsNode(node)){ impl->preprocessingNodeGroup->addChild(node); } } bool SceneView::removePreprocessingNode(osg::Node* node) { return impl->preprocessingNodeGroup->removeChild(node); } void SceneViewImpl::onHiPriorityRenderingToggled(bool on) { //viewer->enableHiPriorityRendering(on); } void SceneViewImpl::onCaptureImageButtonClicked() { QFileDialog dialog(MainWindow::instance()); dialog.setWindowTitle(_("Save an image of the scene view")); dialog.setFileMode(QFileDialog::AnyFile); dialog.setViewMode(QFileDialog::List); dialog.setLabelText(QFileDialog::Accept, _("Save")); dialog.setLabelText(QFileDialog::Reject, _("Cancel")); QStringList filters; filters << _("Any files (*)"); dialog.setNameFilters(filters); dialog.setDirectory(lastCaptureFolder); if(!lastCaptureFile.isEmpty()){ dialog.selectFile(lastCaptureFile); } if(dialog.exec()){ QString filename = dialog.selectedFiles().front(); if(!filename.isEmpty()){ saveImage(filename.toLocal8Bit().data()); lastCaptureFile = filename; lastCaptureFolder = dialog.directory().absolutePath(); } } } bool SceneView::saveImage(const std::string& filename) { return impl->saveImage(filename); } bool SceneViewImpl::saveImage(const std::string& filename) { const bool USE_READ_PIXELS = true; osg::ref_ptr image = new osg::Image; int width = viewer->width(); int height = viewer->height(); if(!USE_READ_PIXELS){ image->allocateImage(width, height, 24, GL_RGB, GL_UNSIGNED_BYTE); camera->attach(osg::Camera::COLOR_BUFFER, image.get()); } viewer->graphicsWindow()->makeCurrentImplementation(); viewer->renderingTraversals(); if(USE_READ_PIXELS){ image->readPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE); } return osgDB::writeImageFile(*image, filename); } void SceneView::setScreenSize(int width, int height) { impl->setScreenSize(width, height); } void SceneViewImpl::setScreenSize(int width, int height) { QRect r = self->geometry(); viewer->setGeometry((r.width() - width) / 2, (r.height() - height) / 2, width, height); } bool SceneView::storeState(Archive& archive) { return impl->storeState(archive); } bool SceneViewImpl::storeState(Archive& archive) { archive.write("mode", (operationMode == SceneObject::VIEW_MODE) ? "view" : "edit"); archive.write("floorGird", floorGridToggle->isChecked()); archive.write("collisions", collisionToggle->isChecked()); archive.write("shadow", shadowCheck->isChecked()); setup.storeState(archive); YamlMapping& cameraData = *archive.createMapping("camera"); if(projectionMode == PERSPECTIVE){ cameraData.write("projection", "perspetive"); orgPerspectiveProjectionMatrix = camera->getProjectionMatrix(); } else { cameraData.write("projection", "ortho"); orgOrthoProjectionMatrix = camera->getProjectionMatrix(); } double p[6]; if(orgPerspectiveProjectionMatrix.getPerspective(p[0], p[1], p[2], p[3])){ YamlSequence& perspective = *cameraData.createFlowStyleSequence("perspective"); for(int i=0; i < 4; ++i){ perspective.append(p[i]); } } if(orgOrthoProjectionMatrix.getOrtho(p[0], p[1], p[2], p[3], p[4], p[5])){ YamlSequence& ortho = *cameraData.createFlowStyleSequence("ortho"); for(int i=0; i < 6; ++i){ ortho.append(p[i]); } } const char* vnames[] = { "eye", "center", "up" }; osg::Vec3f v[3]; double distance = 1.0; // use the float version because the older OSG does not have the double version camera->getViewMatrixAsLookAt(v[0], v[1], v[2], distance); for(int i=0; i < 3; ++i){ YamlSequence& vs = *cameraData.createFlowStyleSequence(vnames[i]); for(int j=0; j < 3; ++j){ vs.append(v[i][j]); } } return true; } bool SceneView::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool SceneViewImpl::restoreState(const Archive& archive) { editModeToggle->setChecked(archive.get("mode", "view") == "edit"); bool on; if(archive.read("shadow", on)){ self->toggleShadow(on); } collisionToggle->setChecked(archive.get("collisions", collisionToggle->isChecked())); floorGridToggle->setChecked(archive.get("floorGrid", floorGridToggle->isChecked())); setup.restoreState(archive); const YamlMapping& cameraData = *archive.findMapping("camera"); if(cameraData.isValid()){ const char* vnames[] = { "eye", "center", "up" }; osg::Vec3d v[3]; int i; for(i=0; i < 3; ++i){ const YamlSequence& vs = *cameraData.findSequence(vnames[i]); if(vs.isValid() && vs.size() == 3){ for(int j=0; j < 3; ++j){ v[i][j] = vs[j].toDouble(); } } else { break; } } if(i == 3){ camera->setViewMatrixAsLookAt(v[0], v[1], v[2]); } string projection; if(cameraData.read("projection", projection)){ if(projection == "perspective"){ projectionMode = PERSPECTIVE; perspectiveToggle->setChecked(true); } else if(projection == "ortho"){ projectionMode = ORTHO; perspectiveToggle->setChecked(false); } } const YamlSequence& pers = *cameraData.findSequence("perspective"); if(pers.isValid() && pers.size() == 4){ orgPerspectiveProjectionMatrix.makePerspective( pers[0].toDouble(), pers[1].toDouble(), pers[2].toDouble(), pers[3].toDouble()); } const YamlSequence& o = *cameraData.findSequence("ortho"); if(o.isValid() && o.size() == 6){ orgOrthoProjectionMatrix.makeOrtho( o[0].toDouble(), o[1].toDouble(), o[2].toDouble(), o[3].toDouble(), o[4].toDouble(), o[5].toDouble()); } resetCameraProjection(); viewer->requestRedraw(); } return true; } choreonoid-1.1.0+dfsg/src/Base/SceneView.h000066400000000000000000000031731207742442300202740ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_SCENE_VIEW_H_INCLUDED #define CNOID_GUIBASE_SCENE_VIEW_H_INCLUDED #include #include "exportdecl.h" namespace osg { class Camera; class Node; } namespace cnoid { class ExtensionManager; class OsgViewer; class SceneObject; class SceneViewImpl; class CNOID_EXPORT SceneView : public View { public: static void initialize(ExtensionManager* ext); static SceneView* mainInstance(); SceneView(); virtual ~SceneView(); OsgViewer* viewer(); void requestRedraw(); void addSceneObject(SceneObject* object); void removeSceneObject(SceneObject* object); void toggleFloorGrid(bool on); void toggleShadow(bool on); /** \todo Test to provide this flexible event signal */ //SignalProxy > sigEvent; osg::Camera* getCameraControl(); bool releaseCameraControl(osg::Camera* camera); void addPreprocessingNode(osg::Node* node); bool removePreprocessingNode(osg::Node* node); bool saveImage(const std::string& filename); void setScreenSize(int width, int height); void updateIndicator(const std::string& text); protected: virtual QWidget* indicatorOnInfoBar(); virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); private: friend class SceneViewImpl; SceneViewImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ScrollBar.cpp000066400000000000000000000017271207742442300206250ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "ScrollBar.h" using namespace cnoid; ScrollBar::ScrollBar(QWidget* parent) : QScrollBar(parent) { initialize(); } ScrollBar::ScrollBar(Qt::Orientation orientation, QWidget* parent) : QScrollBar(orientation, parent) { initialize(); } void ScrollBar::initialize() { connect(this, SIGNAL(valueChanged(int)), this, SLOT(onValueChanged(int))); } void ScrollBar::onValueChanged(int value) { sigValueChanged_(value); } DoubleScrollBar::DoubleScrollBar(QWidget* parent) : QScrollBar(parent) { initialize(); } DoubleScrollBar::DoubleScrollBar(Qt::Orientation orientation, QWidget* parent) : QScrollBar(orientation, parent) { initialize(); } void DoubleScrollBar::initialize() { resolution = 10000.0; connect(this, SIGNAL(valueChanged(int)), this, SLOT(onValueChanged(int))); } void DoubleScrollBar::onValueChanged(int value) { sigValueChanged_(value / resolution); } choreonoid-1.1.0+dfsg/src/Base/ScrollBar.h000066400000000000000000000056641207742442300202760ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_SCROLLBAR_H_INCLUDED #define CNOID_GUIBASE_SCROLLBAR_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT ScrollBar : public QScrollBar { Q_OBJECT public: ScrollBar(QWidget* parent = 0); ScrollBar(Qt::Orientation orientation, QWidget* parent = 0); inline SignalProxy< boost::signal > sigValueChanged() { return sigValueChanged_; } private Q_SLOTS: void onValueChanged(int value); private: boost::signal sigValueChanged_; void initialize(); }; class CNOID_EXPORT DoubleScrollBar : public QScrollBar { Q_OBJECT public: DoubleScrollBar(QWidget* parent = 0); DoubleScrollBar(Qt::Orientation orientation, QWidget* parent = 0); inline double getMaximum() const { return QScrollBar::maximum() / resolution; } inline double getMinimum() const { return QScrollBar::minimum() / resolution; } inline double getPageStep() const { return QScrollBar::pageStep() / resolution; } inline double getSingleStep() const { return QScrollBar::singleStep() / resolution; } inline double getValue() const { return value() / resolution; } inline void setRange(double min, double max) { QScrollBar::setRange(min * resolution, max * resolution); } inline void setPageStep(double step) { QScrollBar::setPageStep(step * resolution); } inline void setSingleStep(double step) { QScrollBar::setSingleStep(step * resolution); } inline void setValue(double value) { QScrollBar::setValue(value * resolution); } inline SignalProxy< boost::signal > sigValueChanged() { return sigValueChanged_; } private Q_SLOTS: void onValueChanged(int value); private: boost::signal sigValueChanged_; double resolution; void initialize(); // Original int version functions are disabled int maximum() const { return QScrollBar::maximum(); } int minimum() const { return QScrollBar::minimum(); } int pageStep() const { return QScrollBar::pageStep(); } int singleStep() const { return QScrollBar::singleStep(); } int value() const { return QScrollBar::value(); } void setMaximum(int max) { QScrollBar::setMaximum(max); } void setMinimum(int min) { QScrollBar::setMinimum(min); } void setPageStep(int step) { QScrollBar::setPageStep(step); } void setRange(int min, int max) { QScrollBar::setRange(min, max); } void setSingleStep(int step) { QScrollBar::setSingleStep(step); } void setValue(int value) { QScrollBar::setValue(value); } }; } #endif choreonoid-1.1.0+dfsg/src/Base/Separator.h000066400000000000000000000015161207742442300203430ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_SEPARATOR_H_INCLUDED #define CNOID_GUIBASE_SEPARATOR_H_INCLUDED #include #include namespace cnoid { class VSeparator : public QFrame { public: inline VSeparator(QWidget* parent = 0) : QFrame(parent) { setFrameStyle(QFrame::VLine | QFrame::Sunken); } }; class HSeparator : public QFrame { public: inline HSeparator(QWidget* parent = 0) : QFrame(parent) { setFrameStyle(QFrame::HLine | QFrame::Sunken); } }; class HSeparatorBox : public QHBoxLayout { public: HSeparatorBox(QWidget* titleWidget) { addWidget(new HSeparator(), 1); addWidget(titleWidget); addWidget(new HSeparator(), 1); } }; } #endif choreonoid-1.1.0+dfsg/src/Base/SignalProxy.h000066400000000000000000000015071207742442300206620ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_SIGNAL_PROXY_H_INCLUDED #define CNOID_GUIBASE_SIGNAL_PROXY_H_INCLUDED #include namespace cnoid { template class SignalProxy { public: inline SignalProxy() : signal(0) { } inline SignalProxy(SignalType& signal) : signal(&signal) { } inline SignalProxy(const SignalProxy& org) : signal(org.signal) { } inline boost::signals::connection connect(typename SignalType::slot_function_type f){ if(signal){ return signal->connect(f); } else { return boost::signals::connection(); } }; private: SignalProxy& operator=(const SignalProxy& rhs) { } // disabled SignalType* signal; }; } #endif choreonoid-1.1.0+dfsg/src/Base/Slider.cpp000066400000000000000000000007251207742442300201610ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "Slider.h" using namespace cnoid; Slider::Slider(QWidget* parent) : QSlider(parent) { initialize(); } Slider::Slider(Qt::Orientation orientation, QWidget* parent) : QSlider(orientation, parent) { initialize(); } void Slider::initialize() { connect(this, SIGNAL(valueChanged(int)), this, SLOT(onValueChanged(int))); } void Slider::onValueChanged(int value) { sigValueChanged_(value); } choreonoid-1.1.0+dfsg/src/Base/Slider.h000066400000000000000000000013131207742442300176200ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_SLIDER_H_INCLUDED #define CNOID_GUIBASE_SLIDER_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT Slider : public QSlider { Q_OBJECT public: Slider(QWidget* parent = 0); Slider(Qt::Orientation orientation, QWidget* parent = 0); inline SignalProxy< boost::signal > sigValueChanged() { return sigValueChanged_; } private Q_SLOTS: void onValueChanged(int value); private: boost::signal sigValueChanged_; void initialize(); }; } #endif choreonoid-1.1.0+dfsg/src/Base/SpinBox.cpp000066400000000000000000000014141207742442300203150ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "SpinBox.h" using namespace cnoid; SpinBox::SpinBox(QWidget* parent) : QSpinBox(parent) { connect(this, SIGNAL(valueChanged(int)), this, SLOT(onValueChanged(int))); connect(this, SIGNAL(editingFinished()), this, SLOT(onEditingFinishded())); } void SpinBox::onValueChanged(int value) { sigValueChanged_(value); } void SpinBox::onEditingFinishded() { sigEditingFinished_(); } DoubleSpinBox::DoubleSpinBox(QWidget* parent) : QDoubleSpinBox(parent) { connect(this, SIGNAL(valueChanged(double)), this, SLOT(onValueChanged(double))); } void DoubleSpinBox::onValueChanged(double value) { sigValueChanged_(value); } void DoubleSpinBox::onEditingFinishded() { sigEditingFinished_(); } choreonoid-1.1.0+dfsg/src/Base/SpinBox.h000066400000000000000000000030001207742442300177530ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_GUIBASE_SPINBOX_H_INCLUDED #define CNOID_GUIBASE_SPINBOX_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT SpinBox : public QSpinBox { Q_OBJECT public: SpinBox(QWidget* parent = 0); inline SignalProxy< boost::signal > sigValueChanged() { return sigValueChanged_; } inline SignalProxy< boost::signal > sigEditingFinished() { return sigEditingFinished_; } private Q_SLOTS: void onValueChanged(int value); void onEditingFinishded(); private: boost::signal sigValueChanged_; boost::signal sigEditingFinished_; }; class CNOID_EXPORT DoubleSpinBox : public QDoubleSpinBox { Q_OBJECT public: DoubleSpinBox(QWidget* parent = 0); inline SignalProxy< boost::signal > sigValueChanged() { return sigValueChanged_; } inline SignalProxy< boost::signal > sigEditingFinished() { return sigEditingFinished_; } private Q_SLOTS: void onValueChanged(double value); void onEditingFinishded(); private: boost::signal sigValueChanged_; boost::signal sigEditingFinished_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/TimeBar.cpp000066400000000000000000000457151207742442300202720ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "TimeBar.h" #include "ExtensionManager.h" #include "Archive.h" #include "MessageView.h" #include "MainWindow.h" #include "OptionManager.h" #include "LazyCaller.h" #include "SpinBox.h" #include "Slider.h" #include "Button.h" #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; inline double myNearByInt(double x) { #ifdef Q_OS_WIN32 double u = ceil(x); double l = floor(x); if(fabs(u - x) < fabs(x - l)){ return u; } else { return l; } #else return nearbyint(x); #endif } class SetupDialog : public QDialog { public: SpinBox* frameRateSpin; SpinBox* playbackFrameRateSpin; DoubleSpinBox* playbackSpeedScaleSpin; QCheckBox* beatModeCheck; DoubleSpinBox* tempoSpin; SpinBox* beatcSpin; SpinBox* beatmSpin; DoubleSpinBox* beatOffsetSpin; SetupDialog() : QDialog(MainWindow::instance()) { setWindowTitle(_("Time Bar Setup")); QVBoxLayout* vbox = new QVBoxLayout(); setLayout(vbox); QHBoxLayout* hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(_("Internal frame rate"))); frameRateSpin = new SpinBox(); frameRateSpin->setAlignment(Qt::AlignCenter); frameRateSpin->setRange(1, 9999); hbox->addWidget(frameRateSpin); hbox->addStretch(); vbox->addLayout(hbox); hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(_("Playback frame rate"))); playbackFrameRateSpin = new SpinBox(); playbackFrameRateSpin->setAlignment(Qt::AlignCenter); playbackFrameRateSpin->setRange(0, 999); playbackFrameRateSpin->setValue(50); hbox->addWidget(playbackFrameRateSpin); hbox->addStretch(); vbox->addLayout(hbox); hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(_("Playback speed scale"))); playbackSpeedScaleSpin = new DoubleSpinBox(); playbackSpeedScaleSpin->setAlignment(Qt::AlignCenter); playbackSpeedScaleSpin->setDecimals(1); playbackSpeedScaleSpin->setRange(0.1, 9.9); playbackSpeedScaleSpin->setSingleStep(0.1); playbackSpeedScaleSpin->setValue(1.0); hbox->addWidget(playbackSpeedScaleSpin); hbox->addStretch(); vbox->addLayout(hbox); /* hbox = new QHBoxLayout(); vbox->addLayout(hbox); beatModeCheck = new QCheckBox(_("Beat mode")); hbox->addWidget(beatModeCheck); beatcSpin = new SpinBox(); beatcSpin->setRange(1, 99); hbox->addWidget(beatcSpin); hbox->addWidget(new QLabel("/")); beatmSpin = new SpinBox(); beatmSpin->setRange(1, 99); hbox->addWidget(beatmSpin); hbox->addStretch(); hbox = new QHBoxLayout(); vbox->addLayout(hbox); hbox->addWidget(new QLabel(_("Tempo"))); tempoSpin = new DoubleSpinBox(); tempoSpin->setRange(1.0, 999.99); tempoSpin->setDecimals(2); hbox->addWidget(tempoSpin); hbox->addWidget(new QLabel(_("Offset"))); beatOffsetSpin = new DoubleSpinBox(); beatOffsetSpin->setRange(-9.99, 9.99); beatOffsetSpin->setDecimals(2); beatOffsetSpin->setSingleStep(0.1); hbox->addWidget(beatOffsetSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox->addStretch(); */ PushButton* okButton = new PushButton(_("&OK")); okButton->setDefault(true); vbox->addStretch(); } }; } namespace cnoid { class TimeBarImpl : public QObject { public: TimeBarImpl(TimeBar* self); ~TimeBarImpl(); bool setTime(double time, bool calledFromPlaybackLoop, QWidget* callerWidget = 0); void onTimeSpinChanged(double value); bool onTimeSliderChangeValue(double value); void setTimeRange(double minTime, double maxTime); void setFrameRate(double rate); void updateTimeProperties(bool forceUpdate); void onPlaybackSpeedScaleChanged(); void onPlaybackFrameRateChanged(); void onPlayActivated(); void onResumeActivated(); void startPlayback(); void stopPlayback(bool isStoppedManually); int startFillLevelUpdate(); void updateFillLevel(int id, double time); void updateMinFillLevel(); void stopFillLevelUpdate(int id); void onTimeRangeSpinsChanged(); void onFrameRateSpinChanged(); virtual void timerEvent(QTimerEvent* event); void onRefreshButtonClicked(); bool storeState(Archive& archive); bool restoreState(const Archive& archive); TimeBar* self; ostream& os; SetupDialog setup; ToolButton* stopResumeButton; ToolButton* frameModeToggle; QIcon resumeIcon; QIcon stopIcon; DoubleSpinBox* timeSpin; Slider* timeSlider; DoubleSpinBox* minTimeSpin; DoubleSpinBox* maxTimeSpin; int decimals; double minTime; double maxTime; double playbackSpeedScale; double playbackFrameRate; double animationTimeOffset; int timerId; QTime timer; bool repeatMode; bool isDoingPlayback; map fillLevelMap; double fillLevel; bool isFillLevelActive; boost::signal sigPlaybackInitialized; boost::signal sigPlaybackStarted; boost::signal sigTimeChanged; boost::signal sigPlaybackStopped; }; } static void onSigOptionsParsed(boost::program_options::variables_map& v) { if(v.count("start-playback")){ callLater(bind(&TimeBar::startPlayback, TimeBar::instance())); } } void TimeBar::initialize(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ext->addToolBar(TimeBar::instance()); ext->optionManager().addOption("start-playback", "start playback automatically"); ext->optionManager().sigOptionsParsed().connect(onSigOptionsParsed); initialized = true; } } TimeBar* TimeBar::instance() { static TimeBar* timeBar = new TimeBar(); return timeBar; } TimeBar::TimeBar() : ToolBar(N_("TimeBar")) { impl = new TimeBarImpl(this); } TimeBarImpl::TimeBarImpl(TimeBar* self) : self(self), os(MessageView::mainInstance()->cout()), resumeIcon(QIcon(":/Base/icons/resume.png")), stopIcon(QIcon(":/Base/icons/stop.png")) { self->setStretchable(true); self->time_ = 0.0; self->frameRate_ = 100.0; decimals = 2; minTime = 0.0; maxTime = 30.0; repeatMode = false; timerId = 0; isDoingPlayback = false; fillLevel = 0; isFillLevelActive = false; self->addButton(QIcon(":/Base/icons/play.png"), _("Start animation")) ->sigClicked().connect(bind(&TimeBarImpl::onPlayActivated, this)); stopResumeButton = self->addButton(resumeIcon, _("Resume animation")); stopResumeButton->setIcon(resumeIcon); stopResumeButton->sigClicked().connect(bind(&TimeBarImpl::onResumeActivated, this)); self->addButton(QIcon(":/Base/icons/refresh.png"), _("Refresh state at the current time")) ->sigClicked().connect(bind(&TimeBarImpl::onRefreshButtonClicked, this)); timeSpin = new DoubleSpinBox(); timeSpin->setAlignment(Qt::AlignCenter); timeSpin->sigValueChanged().connect(bind(&TimeBarImpl::onTimeSpinChanged, this, _1)); self->addWidget(timeSpin); timeSlider = new Slider(Qt::Horizontal); timeSlider->sigValueChanged().connect(bind(&TimeBarImpl::onTimeSliderChangeValue, this, _1)); self->addWidget(timeSlider); minTimeSpin = new DoubleSpinBox(); minTimeSpin->setAlignment(Qt::AlignCenter); minTimeSpin->setRange(-999.0, 999.0); minTimeSpin->sigValueChanged().connect(bind(&TimeBarImpl::onTimeRangeSpinsChanged, this)); self->addWidget(minTimeSpin); self->addLabel(" : "); maxTimeSpin = new DoubleSpinBox(); maxTimeSpin->setAlignment(Qt::AlignCenter); maxTimeSpin->setRange(-999.0, 999.0); maxTimeSpin->sigValueChanged().connect(bind(&TimeBarImpl::onTimeRangeSpinsChanged, this)); self->addWidget(maxTimeSpin); self->addButton(QIcon(":/Base/icons/setup.png"), _("Open the setup dialog")) ->sigClicked().connect(bind(&QDialog::show, &setup)); setup.frameRateSpin->sigValueChanged().connect(bind(&TimeBarImpl::onFrameRateSpinChanged, this)); setup.playbackFrameRateSpin->sigValueChanged().connect(bind(&TimeBarImpl::onPlaybackFrameRateChanged, this)); setup.playbackSpeedScaleSpin->sigValueChanged().connect(bind(&TimeBarImpl::onPlaybackSpeedScaleChanged, this)); playbackSpeedScale = setup.playbackSpeedScaleSpin->value(); playbackFrameRate = setup.playbackFrameRateSpin->value(); updateTimeProperties(true); } TimeBar::~TimeBar() { delete impl; } TimeBarImpl::~TimeBarImpl() { } SignalProxy< boost::signal > TimeBar::sigPlaybackInitialized() { return impl->sigPlaybackInitialized; } SignalProxy< boost::signal > TimeBar::sigPlaybackStarted() { return impl->sigPlaybackStarted; } /** Signal emitted when the TimeBar's time changes. In the function connected to this signal, please return true if the time is valid for it, and return false if the time is not valid. The example of the latter case is that the time is over the length of the data processed in the function. */ SignalProxy< boost::signal > TimeBar::sigTimeChanged() { return impl->sigTimeChanged; } SignalProxy< boost::signal > TimeBar::sigPlaybackStopped() { return impl->sigPlaybackStopped; } bool TimeBar::setTime(double time) { return impl->setTime(time, false); } /** @todo check whether block() and unblock() of sigc::connection decrease the performance or not. */ bool TimeBarImpl::setTime(double time, bool calledFromPlaybackLoop, QWidget* callerWidget) { if(TRACE_FUNCTIONS){ cout << "TimeBarImpl::setTime(" << time << ", " << calledFromPlaybackLoop << ")" << endl; } if(!calledFromPlaybackLoop && isDoingPlayback){ return false; } const double newTime = myNearByInt(time * self->frameRate_) / self->frameRate_; if(newTime == self->time_){ if(calledFromPlaybackLoop){ return true; } if(callerWidget){ return false; } } self->time_ = newTime; if(callerWidget != timeSpin){ timeSpin->blockSignals(true); timeSpin->setValue(self->time_); timeSpin->blockSignals(false); } if(callerWidget != timeSlider){ timeSlider->blockSignals(true); timeSlider->setValue((int)myNearByInt(self->time_ * pow(10.0, decimals))); timeSlider->blockSignals(false); } return sigTimeChanged(self->time_); } void TimeBarImpl::onTimeSpinChanged(double value) { if(TRACE_FUNCTIONS){ cout << "TimeBarImpl::onTimeSpinChanged()" << endl; } setTime(value, false, timeSpin); } bool TimeBarImpl::onTimeSliderChangeValue(double value) { if(TRACE_FUNCTIONS){ cout << "TimeBarImpl::onTimeSliderChanged(): value = " << value << endl; } setTime(value / pow(10.0, decimals), false, timeSlider); return true; } void TimeBar::setFrameRate(double rate) { impl->setFrameRate(rate); } void TimeBarImpl::setFrameRate(double rate) { if(rate > 0.0){ self->frameRate_ = rate; updateTimeProperties(false); } } double TimeBar::minTime() const { return impl->minTime; } double TimeBar::maxTime() const { return impl->maxTime; } void TimeBar::setTimeRange(double min, double max) { impl->setTimeRange(min, max); } void TimeBarImpl::setTimeRange(double minTime, double maxTime) { this->minTime = minTime; this->maxTime = std::max(minTime, maxTime); updateTimeProperties(false); } void TimeBarImpl::updateTimeProperties(bool forceUpdate) { timeSpin->blockSignals(true); timeSlider->blockSignals(true); minTimeSpin->blockSignals(true); maxTimeSpin->blockSignals(true); setup.frameRateSpin->blockSignals(true); double timeStep = 1.0 / self->frameRate_; decimals = static_cast(ceil(log10(self->frameRate_))); double r = pow(10.0, decimals); if(forceUpdate || (minTime != timeSpin->minimum() || maxTime != timeSpin->maximum())){ timeSpin->setRange(minTime, maxTime); timeSlider->setRange((int)myNearByInt(minTime * r), (int)myNearByInt(maxTime * r)); } timeSpin->setDecimals(decimals); timeSpin->setSingleStep(timeStep); timeSlider->setSingleStep(timeStep * r); minTimeSpin->setValue(minTime); maxTimeSpin->setValue(maxTime); setup.frameRateSpin->setValue(self->frameRate_); setup.frameRateSpin->blockSignals(false); maxTimeSpin->blockSignals(false); minTimeSpin->blockSignals(false); timeSlider->blockSignals(false); timeSpin->blockSignals(false); setTime(self->time_, false); } void TimeBarImpl::onPlaybackSpeedScaleChanged() { playbackSpeedScale = setup.playbackSpeedScaleSpin->value(); if(isDoingPlayback){ startPlayback(); } } double TimeBar::playbackSpeedScale() const { return impl->setup.playbackSpeedScaleSpin->value(); } void TimeBarImpl::onPlaybackFrameRateChanged() { playbackFrameRate = setup.playbackFrameRateSpin->value(); if(isDoingPlayback){ startPlayback(); } } double TimeBar::playbackFrameRate() const { return impl->setup.playbackFrameRateSpin->value(); } void TimeBar::setRepeatMode(bool on) { impl->repeatMode = on; } void TimeBarImpl::onPlayActivated() { stopPlayback(true); setTime(minTime, false); startPlayback(); } void TimeBarImpl::onResumeActivated() { if(isDoingPlayback){ stopPlayback(true); } else { stopPlayback(true); startPlayback(); } } void TimeBar::startPlayback() { impl->startPlayback(); } void TimeBarImpl::startPlayback() { stopPlayback(false); animationTimeOffset = self->time_; if(sigPlaybackInitialized(self->time_)){ sigPlaybackStarted(self->time_); if(!setTime(self->time_, true)){ sigPlaybackStopped(self->time_, false); } else { isDoingPlayback = true; const static QString tip(_("Stop animation")); stopResumeButton->setIcon(stopIcon); stopResumeButton->setToolTip(tip); timerId = startTimer((int)myNearByInt(1000.0 / playbackFrameRate)); timer.start(); } } } void TimeBar::stopPlayback(bool isStoppedManually) { impl->stopPlayback(isStoppedManually); } void TimeBarImpl::stopPlayback(bool isStoppedManually) { if(isDoingPlayback){ killTimer(timerId); isDoingPlayback = false; sigPlaybackStopped(self->time_, isStoppedManually); const static QString tip(_("Resume animation")); stopResumeButton->setIcon(resumeIcon); stopResumeButton->setToolTip(tip); if(fillLevelMap.empty()){ isFillLevelActive = false; } } } bool TimeBar::isDoingPlayback() { return impl->isDoingPlayback; } int TimeBar::startFillLevelUpdate() { return impl->startFillLevelUpdate(); } int TimeBarImpl::startFillLevelUpdate() { int id=0; if(fillLevelMap.empty()){ isFillLevelActive = true; } else { while(fillLevelMap.find(id) == fillLevelMap.end()){ ++id; } } updateFillLevel(id, 0.0); return id; } void TimeBar::updateFillLevel(int id, double time) { impl->updateFillLevel(id, time); } void TimeBarImpl::updateFillLevel(int id, double time) { fillLevelMap[id] = time; updateMinFillLevel(); } void TimeBarImpl::updateMinFillLevel() { double minFillLevel = std::numeric_limits::max(); map::iterator p; for(p = fillLevelMap.begin(); p != fillLevelMap.end(); ++p){ minFillLevel = std::min(p->second, minFillLevel); } fillLevel = minFillLevel; } void TimeBar::stopFillLevelUpdate(int id) { impl->stopFillLevelUpdate(id); } void TimeBarImpl::stopFillLevelUpdate(int id) { fillLevelMap.erase(id); if(!fillLevelMap.empty()){ updateMinFillLevel(); } else { if(!isDoingPlayback){ isFillLevelActive = false; } } } void TimeBarImpl::timerEvent(QTimerEvent* event) { double time = animationTimeOffset + playbackSpeedScale * (timer.elapsed() / 1000.0); bool doStopAtLastFillLevel = false; if(isFillLevelActive){ if(time > fillLevel){ animationTimeOffset -= (time - fillLevel); time = fillLevel; if(fillLevelMap.empty()){ doStopAtLastFillLevel = true; } } } if(!setTime(time, true) || doStopAtLastFillLevel){ stopPlayback(false); if(!doStopAtLastFillLevel && repeatMode){ setTime(minTime, true); startPlayback(); } } } void TimeBarImpl::onTimeRangeSpinsChanged() { setTimeRange(minTimeSpin->value(), maxTimeSpin->value()); } void TimeBarImpl::onFrameRateSpinChanged() { setFrameRate(setup.frameRateSpin->value()); } void TimeBarImpl::onRefreshButtonClicked() { if(!isDoingPlayback){ sigTimeChanged(self->time_); } } QSize TimeBar::minimumSizeHint () const { QSize s = ToolBar::minimumSizeHint(); s.setWidth(s.width() * 2); return s; } QSize TimeBar::sizeHint () const { QSize s = ToolBar::minimumSizeHint(); s.setWidth(s.width() * 3); return s; } bool TimeBar::storeState(Archive& archive) { return impl->storeState(archive); } bool TimeBarImpl::storeState(Archive& archive) { archive.write("minTime", minTime); archive.write("maxTime", maxTime); archive.write("frameRate", self->frameRate_); archive.write("currentTime", self->time_); return true; } bool TimeBar::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool TimeBarImpl::restoreState(const Archive& archive) { archive.read("minTime", minTime); archive.read("maxTime", maxTime); archive.read("frameRate", self->frameRate_); archive.read("currentTime", self->time_); updateTimeProperties(false); return true; } choreonoid-1.1.0+dfsg/src/Base/TimeBar.h000066400000000000000000000066261207742442300177350ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_TIME_BAR_H_INCLUDED #define CNOID_GUIBASE_TIME_BAR_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { class ExtensionManager; class TimeBarImpl; class CNOID_EXPORT TimeBar : public ToolBar { class LogicalSum { public: typedef bool result_type; template bool operator()(InputIterator first, InputIterator last) const { bool result = false; while(first != last){ result |= *first++; } return result; } }; class LogicalProduct { public: typedef bool result_type; template bool operator()(InputIterator first, InputIterator last) const { bool result = true; while(first != last){ result &= *first++; } return result; } }; public: static void initialize(ExtensionManager* ext); static TimeBar* instance(); /** \note If any connected slot returns false, the playback is canceled. */ SignalProxy< boost::signal > sigPlaybackInitialized(); SignalProxy< boost::signal > sigPlaybackStarted(); SignalProxy< boost::signal > sigTimeChanged(); SignalProxy< boost::signal > sigPlaybackStopped(); inline double time() const { return time_; } bool setTime(double time); double minTime() const; double maxTime() const; void setTimeRange(double min, double max); inline double frameRate() const { return frameRate_; } void setFrameRate(double rate); inline double timeStep() const { return 1.0 / frameRate_; } inline bool isBeatMode() const { return isBeatMode_; } inline double beatOffset() const { return beatOffset_; } inline double tempo() const { return tempo_; } double timeOfBeatLocation(double beatLocation) const; double beatLocationOfTime(double time) const; inline int beatNumerator() const { return beatNumerator_; } inline int beatDenominator() const { return beatDenominator_; } double playbackSpeedScale() const; double playbackFrameRate() const; void setRepeatMode(bool on); void startPlayback(); void stopPlayback(bool isStoppedManually = false); bool isDoingPlayback(); int startFillLevelUpdate(); void updateFillLevel(int id, double time); void stopFillLevelUpdate(int id); virtual QSize minimumSizeHint () const; virtual QSize sizeHint () const; protected: virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); private: TimeBar(); virtual ~TimeBar(); TimeBarImpl* impl; double time_; double frameRate_; bool isBeatMode_; double beatOffset_; double tempo_; int beatNumerator_; int beatDenominator_; friend class TimeBarImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/TimeSyncItemEngineManager.cpp000066400000000000000000000054621207742442300237350ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "TimeSyncItemEngineManager.h" #include "ItemTreeView.h" #include "TimeBar.h" #include "LazyCaller.h" #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { double currentTime; typedef vector FactoryArray; typedef map FactoryArrayMap; FactoryArrayMap allFactories; vector engines; signals::connection connectionOfSelectionOrTreeChanged; signals::connection connectionOfTimeChanged; LazyCaller updateCaller; LazyCaller resetCaller; bool setTime(double time) { bool isActive = false; currentTime = time; for(size_t i=0; i < engines.size(); ++i){ isActive |= engines[i]->onTimeChanged(time); } return isActive; } void update() { setTime(currentTime); } void onItemSelectionOrTreeChanged(const ItemList& selectedItems) { engines.clear(); for(size_t i=0; i < selectedItems.size(); ++i){ Item* sourceItem = selectedItems[i]; for(FactoryArrayMap::iterator p = allFactories.begin(); p != allFactories.end(); ++p){ FactoryArray& factories = p->second; for(FactoryArray::iterator q = factories.begin(); q != factories.end(); ++q){ TimeSyncItemEngineFactoryFunc& factoryFunc = *q; TimeSyncItemEnginePtr engine = factoryFunc(sourceItem); if(engine){ engines.push_back(engine); } } } } setTime(currentTime); } void reset() { onItemSelectionOrTreeChanged(ItemTreeView::mainInstance()->selectedItems()); } } void TimeSyncItemEngine::notifyUpdate() { updateCaller.request(); } void TimeSyncItemEngineManager::initialize() { static bool initialized = false; if(!initialized){ currentTime = 0.0; connectionOfSelectionOrTreeChanged = ItemTreeView::mainInstance()->sigSelectionOrTreeChanged().connect(onItemSelectionOrTreeChanged); connectionOfTimeChanged = TimeBar::instance()->sigTimeChanged().connect(setTime); updateCaller.set(update, IDLE_PRIORITY_LOW); initialized = true; } } TimeSyncItemEngineManager::TimeSyncItemEngineManager(const std::string& moduleName) : moduleName(moduleName) { } TimeSyncItemEngineManager::~TimeSyncItemEngineManager() { allFactories.erase(moduleName); engines.clear(); resetCaller.request(); } void TimeSyncItemEngineManager::addEngineFactory(TimeSyncItemEngineFactoryFunc factoryFunc) { allFactories[moduleName].push_back(factoryFunc); } choreonoid-1.1.0+dfsg/src/Base/TimeSyncItemEngineManager.h000066400000000000000000000020671207742442300234000ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_TIME_SYNC_ITEM_ENGINE_MANAGER_H_INCLUDED #define CNOID_GUIBASE_TIME_SYNC_ITEM_ENGINE_MANAGER_H_INCLUDED #include "Item.h" #include #include #include "exportdecl.h" namespace cnoid { class AppImpl; class CNOID_EXPORT TimeSyncItemEngine : public Referenced, boost::signals::trackable { public: virtual bool onTimeChanged(double time) = 0; protected: void notifyUpdate(); }; typedef boost::intrusive_ptr TimeSyncItemEnginePtr; typedef boost::function TimeSyncItemEngineFactoryFunc; class CNOID_EXPORT TimeSyncItemEngineManager { public: static void initialize(); TimeSyncItemEngineManager(const std::string& moduleName); ~TimeSyncItemEngineManager(); void addEngineFactory(TimeSyncItemEngineFactoryFunc factoryFunc); private: std::string moduleName; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ToolBar.cpp000066400000000000000000000163651207742442300203100ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ToolBar.h" #include "ToolBarArea.h" #include "MainWindow.h" #include "Separator.h" #include #include #include using namespace std; using namespace cnoid; namespace { class ToolBarHandle : public QWidget { ToolBar* toolBar; bool isDragging; QPoint dragOrgLocalPos; public: ToolBarHandle(ToolBar* toolBar) : QWidget(toolBar), toolBar(toolBar) { setFixedWidth(12); isDragging = false; setCursor(Qt::OpenHandCursor); } virtual void paintEvent(QPaintEvent* event) { QStylePainter painter(this); QStyleOptionToolBar option; option.initFrom(this); option.features = QStyleOptionToolBar::Movable; option.toolBarArea = Qt::TopToolBarArea; option.state = QStyle::State_Horizontal; option.rect.setHeight(height()); option.rect.setWidth(16); painter.drawPrimitive(QStyle::PE_IndicatorToolBarHandle, option); } virtual void mousePressEvent(QMouseEvent* event) { if(event->button() == Qt::LeftButton){ dragOrgLocalPos = event->pos(); setCursor(Qt::ClosedHandCursor); isDragging = true; } } virtual void mouseMoveEvent(QMouseEvent* event) { if(isDragging){ toolBar->toolBarArea()->dragToolBar(toolBar, event->globalPos() - dragOrgLocalPos); } } virtual void mouseReleaseEvent(QMouseEvent* event) { setCursor(Qt::OpenHandCursor); isDragging = false; } }; } ToolBar::ToolBar(const QString& title) { setWindowTitle(title); setObjectName(title); mainWindow = MainWindow::instance(); hbox = new QHBoxLayout(this); hbox->setSpacing(0); hbox->setContentsMargins(0, 0, 0, 0); setLayout(hbox); handle = new ToolBarHandle(this); hbox->addWidget(handle); hbox->addSpacing(4); radioGroup = 0; isNewRadioGroupRequested = true; toolBarArea_ = 0; desiredX = 0; layoutPriority = 0; isStretchable_ = false; connect(mainWindow, SIGNAL(iconSizeChanged(const QSize&)), this, SLOT(changeIconSize(const QSize&))); } ToolBar::~ToolBar() { } ToolButton* ToolBar::addButton(const QString& text, const QString& tooltip) { ToolButton* button = new ToolButton(this); button->setText(text); button->setAutoRaise(true); button->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); if(!tooltip.isEmpty()){ button->setToolTip(tooltip); } hbox->addWidget(button); return button; } ToolButton* ToolBar::addButton(const QIcon& icon, const QString& tooltip) { ToolButton* button = new ToolButton(this); button->setIconSize(mainWindow->iconSize()); button->setIcon(icon); button->setAutoRaise(true); button->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); if(!tooltip.isEmpty()){ button->setToolTip(tooltip); } hbox->addWidget(button); return button; } ToolButton* ToolBar::addButton(const char* const* xpm, const QString& tooltip) { return addButton(QIcon(QPixmap(xpm)), tooltip); } ToolButton* ToolBar::addToggleButton(const QString& text, const QString& tooltip) { ToolButton* button = addButton(text, tooltip); button->setCheckable(true); return button; } ToolButton* ToolBar::addToggleButton(const QIcon& icon, const QString& tooltip) { ToolButton* button = addButton(icon, tooltip); button->setCheckable(true); return button; } ToolButton* ToolBar::addToggleButton(const char* const* xpm, const QString& tooltip) { ToolButton* button = addButton(xpm, tooltip); button->setCheckable(true); return button; } void ToolBar::requestNewRadioGroup() { radioGroup = 0; isNewRadioGroupRequested = true; } QButtonGroup* ToolBar::currentRadioGroup() { if(isNewRadioGroupRequested){ radioGroup = new QButtonGroup(this); isNewRadioGroupRequested = false; } return radioGroup; } void ToolBar::setRadioButton(ToolButton* button) { button->setCheckable(true); currentRadioGroup()->addButton(button); } ToolButton* ToolBar::addRadioButton(const QString& text, const QString& tooltip) { ToolButton* button = addButton(text, tooltip); setRadioButton(button); return button; } ToolButton* ToolBar::addRadioButton(const QIcon& icon, const QString& tooltip) { ToolButton* button = addButton(icon, tooltip); setRadioButton(button); return button; } ToolButton* ToolBar::addRadioButton(const char* const* xpm, const QString& tooltip) { ToolButton* button = addButton(xpm, tooltip); setRadioButton(button); return button; } void ToolBar::addAction(QAction* action) { ToolButton* button = new ToolButton(this); button->setAutoRaise(true); button->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); button->setDefaultAction(action); } void ToolBar::addWidget(QWidget* widget) { hbox->addWidget(widget); } QLabel* ToolBar::addLabel(const QString& text) { QLabel* label = new QLabel(text, this); hbox->addWidget(label); return label; } QLabel* ToolBar::addImage(const QString& filename) { QLabel* label = new QLabel(this); label->setPixmap(QPixmap(filename)); hbox->addWidget(label); return label; } QWidget* ToolBar::addSeparator(int spacing) { VSeparator* sep = new VSeparator(this); hbox->addSpacing(spacing); hbox->addWidget(sep); hbox->addSpacing(spacing); return sep; } void ToolBar::addSpacing(int size) { hbox->addSpacing(size); } void ToolBar::setEnabled(bool on) { /* if(on){ QWidget::setEnabled(on); } else { int n = hbox->count(); for(int i=0; i < n; ++i){ QLayoutItem* item = hbox->itemAt(i); QWidget* widget = item->widget(); if(widget){ widget->setEnabled(false); } handle->setEnabled(true); } } */ int n = hbox->count(); for(int i=0; i < n; ++i){ QLayoutItem* item = hbox->itemAt(i); QWidget* widget = item->widget(); if(widget){ widget->setEnabled(on); } handle->setEnabled(true); } } void ToolBar::changeIconSize(const QSize& iconSize) { changeIconSizeSub(hbox, iconSize); } void ToolBar::changeIconSizeSub(QLayout* layout, const QSize& iconSize) { int n = layout->count(); for(int i=0; i < n; ++i){ QLayoutItem* item = layout->itemAt(i); QLayout * childLayout = dynamic_cast(item); if(childLayout){ changeIconSizeSub(childLayout, iconSize); } QWidgetItem* widgetItem = dynamic_cast(item); if(widgetItem){ QWidget* widget = widgetItem->widget(); ToolButton* button = dynamic_cast(widget); if(button){ button->setIconSize(mainWindow->iconSize()); } } } } bool ToolBar::storeState(Archive& archive) { return true; } bool ToolBar::restoreState(const Archive& archive) { return true; } choreonoid-1.1.0+dfsg/src/Base/ToolBar.h000066400000000000000000000054551207742442300177530ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_TOOL_BAR_H_INCLUDED #define CNOID_GUIBASE_TOOL_BAR_H_INCLUDED #include "Button.h" #include #include #include #include #include "exportdecl.h" namespace cnoid { class Archive; class ExtensionManager; class ToolBarArea; class MainWindow; class CNOID_EXPORT ToolBar : public QWidget { Q_OBJECT public: ToolBar(const QString& title); virtual ~ToolBar(); ToolButton* addButton(const QString& text, const QString& tooltip = QString()); ToolButton* addButton(const QIcon& icon, const QString& tooltip = QString()); ToolButton* addButton(const char* const* xpm, const QString& tooltip = QString()); ToolButton* addToggleButton(const QString& text, const QString& tooltip = QString()); ToolButton* addToggleButton(const QIcon& icon, const QString& tooltip = QString()); ToolButton* addToggleButton(const char* const* xpm, const QString& tooltip = QString()); void requestNewRadioGroup(); QButtonGroup* currentRadioGroup(); ToolButton* addRadioButton(const QString& text, const QString& tooltip = QString()); ToolButton* addRadioButton(const QIcon& icon, const QString& tooltip = QString()); ToolButton* addRadioButton(const char* const* xpm, const QString& tooltip = QString()); void addAction(QAction* action); void addWidget(QWidget* widget); QLabel* addLabel(const QString& text); QLabel* addImage(const QString& filename); QWidget* addSeparator(int spacing = 0); void addSpacing(int size); inline void setStretchable(bool on) { isStretchable_ = on; } inline bool isStretchable() { return isStretchable_; } inline ToolBarArea* toolBarArea() { return toolBarArea_; } class LayoutPriorityCmp { public: inline bool operator() (ToolBar* bar1, ToolBar* bar2) { return (bar1->layoutPriority < bar2->layoutPriority); } }; virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); public Q_SLOTS: void setEnabled(bool on); void changeIconSize(const QSize& iconSize); private: QHBoxLayout* hbox; QWidget* handle; QButtonGroup* radioGroup; bool isNewRadioGroupRequested; MainWindow* mainWindow; ToolBarArea* toolBarArea_; // used for layouting tool bars on a ToolBarArea bool isStretchable_; int desiredX; int layoutPriority; void setRadioButton(ToolButton* button); friend class ToolBarAreaImpl; void changeIconSizeSub(QLayout* layout, const QSize& iconSize); }; } #endif choreonoid-1.1.0+dfsg/src/Base/ToolBar.old.cpp000066400000000000000000000057071207742442300210630ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ToolBar.h" #include using namespace std; using namespace cnoid; ToolBar::ToolBar(const QString& title) : QToolBar(title) { setObjectName(title); radioGroup = 0; isNewRadioGroupRequested = true; toolBarArea = 0; desiredX = 0; layoutPriority = 0; isStretchable_ = false; } ToolBar::~ToolBar() { } Action* ToolBar::addButton(const QString& text, const QString& tooltip) { Action* action = new Action(text, this); if(!tooltip.isEmpty()){ action->setToolTip(tooltip); } addAction(action); return action; } Action* ToolBar::addButton(const QIcon& icon, const QString& tooltip) { Action* action = new Action(icon, this); if(!tooltip.isEmpty()){ action->setToolTip(tooltip); } addAction(action); return action; } Action* ToolBar::addButton(const char* const* xpm, const QString& tooltip) { return addButton(QIcon(QPixmap(xpm)), tooltip); } Action* ToolBar::addToggleButton(const QString& text, const QString& tooltip) { Action* action = addButton(text, tooltip); action->setCheckable(true); return action; } Action* ToolBar::addToggleButton(const QIcon& icon, const QString& tooltip) { Action* action = addButton(icon, tooltip); action->setCheckable(true); return action; } Action* ToolBar::addToggleButton(const char* const* xpm, const QString& tooltip) { Action* action = addButton(xpm, tooltip); action->setCheckable(true); return action; } void ToolBar::requestNewRadioGroup() { radioGroup = 0; isNewRadioGroupRequested = true; } QActionGroup* ToolBar::currentRadioGroup() { if(isNewRadioGroupRequested){ radioGroup = new QActionGroup(this); isNewRadioGroupRequested = false; } return radioGroup; } void ToolBar::setRadioButton(Action* action) { action->setCheckable(true); action->setActionGroup(currentRadioGroup()); } Action* ToolBar::addRadioButton(const QString& text, const QString& tooltip) { Action* action = addButton(text, tooltip); setRadioButton(action); return action; } Action* ToolBar::addRadioButton(const QIcon& icon, const QString& tooltip) { Action* action = addButton(icon, tooltip); setRadioButton(action); return action; } Action* ToolBar::addRadioButton(const char* const* xpm, const QString& tooltip) { Action* action = addButton(xpm, tooltip); setRadioButton(action); return action; } QLabel* ToolBar::addLabel(const QString& text) { QLabel* label = new QLabel(text, this); addWidget(label); return label; } void ToolBar::addSpace(int size) { } int ToolBar::bestStretchWidth() { int bestWidth = sizeHint().width(); if(isStretchable() && bestWidth > 0){ bestWidth = bestWidth * 2; } return bestWidth; } bool ToolBar::storeState(Archive& archive) { return true; } bool ToolBar::restoreState(const Archive& archive) { return true; } choreonoid-1.1.0+dfsg/src/Base/ToolBar.old.h000066400000000000000000000043241207742442300205220ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_TOOL_BAR_H_INCLUDED #define CNOID_GUIBASE_TOOL_BAR_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { class Archive; class ExtensionManager; class ToolBarArea; class CNOID_EXPORT ToolBar : public QToolBar { public: ToolBar(const QString& title); virtual ~ToolBar(); Action* addButton(const QString& text, const QString& tooltip = QString()); Action* addButton(const QIcon& icon, const QString& tooltip = QString()); Action* addButton(const char* const* xpm, const QString& tooltip = QString()); Action* addToggleButton(const QString& text, const QString& tooltip = QString()); Action* addToggleButton(const QIcon& icon, const QString& tooltip = QString()); Action* addToggleButton(const char* const* xpm, const QString& tooltip = QString()); void requestNewRadioGroup(); QActionGroup* currentRadioGroup(); Action* addRadioButton(const QString& text, const QString& tooltip = QString()); Action* addRadioButton(const QIcon& icon, const QString& tooltip = QString()); Action* addRadioButton(const char* const* xpm, const QString& tooltip = QString()); QLabel* addLabel(const QString& text); void addSpace(int size); void setStretchable(bool on) { isStretchable_ = on; } bool isStretchable() { return isStretchable_; } virtual int bestStretchWidth(); class LayoutPriorityCmp { public: bool operator() (ToolBar* bar1, ToolBar* bar2) { return (bar1->layoutPriority < bar2->layoutPriority); } }; virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); private: QActionGroup* radioGroup; bool isNewRadioGroupRequested; ToolBarArea* toolBarArea; // used for layouting tool bars on a ToolBarArea bool isStretchable_; int desiredX; int layoutPriority; void setRadioButton(Action* action); friend class ToolBarAreaImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/ToolBarArea.cpp000066400000000000000000000504361207742442300210760ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ToolBarArea.h" #include "Separator.h" #include "ToolBar.h" #include "MainWindow.h" #include "LazyCaller.h" #include #include #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool DEBUG_MODE = false; typedef std::vector ToolBarList; class ToolBarRow { public: ToolBarRow(ToolBarArea* toolBarArea) : toolBarArea(toolBarArea), separator(toolBarArea) { height = 0; separator.hide(); } ToolBarArea* toolBarArea; ToolBarList toolBars; HSeparator separator; int height; }; typedef shared_ptr ToolBarRowPtr; // for archiving struct LayoutState { public: string name; int desiredX; int layoutPriority; }; typedef list RowLayoutState; typedef list AreaLayoutState; } namespace cnoid { class ToolBarAreaImpl { public: ToolBarAreaImpl(ToolBarArea* self); ~ToolBarAreaImpl(); void getAllToolBars(std::vector& bars); void storeLayout(YamlMappingPtr layout); void restoreLayout(const YamlMappingPtr layout); bool addToolBar(ToolBar* toolBar); void removeToolBar(ToolBar* toolBar); void dragToolBar(ToolBar* toolBar, const QPoint& globalPos); void setNewToolBars(); void expandStrechableBars (ToolBarRowPtr& row, int portion, int shift, int barIndex, int numStrechables, int lastSpace); void useSpace(ToolBarRowPtr& row, int space, int shift, int barIndex, int numStrechables, int allspace ); void setNewToolBar(ToolBar* toolBar, vector& numStrechablesOfRow); void layoutToolBars(); void layoutToolBarRow(ToolBarRowPtr toolBarRow, int& io_rowTop, bool isNewBottomRow); void layoutToolBarRowWithDraggedToolBar(ToolBarList& toolBars, int rowTop, int rowHeight); int normalizeLayoutPriorities(ToolBarList& toolBars); void layoutToolBarRowPart(ToolBarList& toolBars, int rowTop, int rowHeight, int partLeft, int partRight); void resizeEvent(QResizeEvent* event); ToolBarArea* self; bool isBeforeDoingInitialLayout; ToolBarList newToolBars; deque toolBarRows; ToolBarRowPtr spilledToolBarRow; YamlMappingPtr initialLayout; int areaHeight; int prevAreaHeight; ToolBar* draggedToolBar; bool draggedToolBarHasNotBeenInserted; int dragY; LazyCaller layoutToolBarsCaller; }; } ToolBarArea::ToolBarArea(QWidget* parent) : QWidget(parent) { impl = new ToolBarAreaImpl(this); } ToolBarAreaImpl::ToolBarAreaImpl(ToolBarArea* self) : self(self), spilledToolBarRow(new ToolBarRow(self)), layoutToolBarsCaller(bind(&ToolBarAreaImpl::layoutToolBars, this)) { isBeforeDoingInitialLayout = true; draggedToolBar = 0; draggedToolBarHasNotBeenInserted = false; areaHeight = 0; prevAreaHeight = 0; } ToolBarArea::~ToolBarArea() { delete impl; } ToolBarAreaImpl::~ToolBarAreaImpl() { } std::vector ToolBarArea::getAllToolBars() { std::vector bars; impl->getAllToolBars(bars); return bars; } void ToolBarAreaImpl::getAllToolBars(std::vector& bars) { bars.clear(); for(size_t i=0; i < toolBarRows.size(); ++i){ ToolBarRowPtr& row = toolBarRows[i]; vector& toolBars = row->toolBars; for(size_t j=0; j < toolBars.size(); ++j){ bars.push_back(toolBars[j]); } } } bool ToolBarArea::addToolBar(ToolBar* toolBar) { return impl->addToolBar(toolBar); } bool ToolBarAreaImpl::addToolBar(ToolBar* toolBar) { if(toolBar){ if(!toolBar->toolBarArea()){ toolBar->toolBarArea_ = self; newToolBars.push_back(toolBar); if(!isBeforeDoingInitialLayout){ layoutToolBarsCaller.request(); } return true; } } return false; } void ToolBarArea::removeToolBar(ToolBar* toolBar) { if(toolBar){ impl->removeToolBar(toolBar); } } void ToolBarAreaImpl::removeToolBar(ToolBar* toolBar) { for(size_t i=0; i < toolBarRows.size(); ++i){ ToolBarRowPtr row = toolBarRows[i]; vector& toolBars = row->toolBars; for(size_t j=0; j < toolBars.size(); ++j){ if(toolBars[j] == toolBar){ toolBar->hide(); toolBar->setParent(0); toolBar->toolBarArea_ = 0; toolBars.erase(toolBars.begin() + j); if(!isBeforeDoingInitialLayout){ layoutToolBarsCaller.request(); } return; } } } } void ToolBarArea::setInitialLayout(YamlMappingPtr layout) { if(impl->isBeforeDoingInitialLayout){ impl->initialLayout = layout; } } void ToolBarArea::resizeEvent(QResizeEvent* event) { QWidget::resizeEvent(event); impl->resizeEvent(event); } void ToolBarAreaImpl::resizeEvent(QResizeEvent* event) { if(DEBUG_MODE){ cout << "ToolBarAreaImpl::resizeEvent(" << event->size().width() << ", " << event->size().height() << ")"; cout << ", isVisible =" << self->isVisible() << endl; } if(isBeforeDoingInitialLayout){ if(!(MainWindow::instance()->windowState() & (Qt::WindowMaximized | Qt::WindowFullScreen)) || self->isVisible()){ isBeforeDoingInitialLayout = false; if(initialLayout){ restoreLayout(initialLayout); initialLayout = 0; } else { layoutToolBars(); } } } else { if(event->size().width() != event->oldSize().width()){ layoutToolBars(); } } } void ToolBarArea::doInitialLayout() { if(DEBUG_MODE){ cout << "ToolBarAreaImpl::doInitialLayout()" << endl; } if(impl->isBeforeDoingInitialLayout){ impl->isBeforeDoingInitialLayout = false; if(impl->initialLayout){ impl->restoreLayout(impl->initialLayout); impl->initialLayout = 0; } else { impl->layoutToolBars(); } } } bool ToolBarArea::event(QEvent* event) { if(event->type() == QEvent::LayoutRequest){ if(DEBUG_MODE){ cout << "ToolBarArea::event(QEvent::LayoutRequest)" << endl; } impl->layoutToolBars(); return true; } return QWidget::event(event); } void ToolBarArea::storeLayout(YamlMappingPtr layout) { impl->storeLayout(layout); } void ToolBarAreaImpl::storeLayout(YamlMappingPtr layout) { YamlMapping* layoutOfToolBars = layout->createMapping("layoutOfToolBars"); YamlSequence* rows = layoutOfToolBars->createSequence("rows"); for(size_t i=0; i < toolBarRows.size(); ++i){ ToolBarList& toolBars = toolBarRows[i]->toolBars; if(!toolBars.empty()){ YamlSequence* bars = new YamlSequence(); for(ToolBarList::iterator p = toolBars.begin(); p != toolBars.end(); ++p){ ToolBar* toolBar = *p; YamlMapping* state = new YamlMapping(); state->setFlowStyle(true); state->write("name", toolBar->objectName().toStdString(), YAML_DOUBLE_QUOTED); state->write("x", toolBar->desiredX); state->write("priority", toolBar->layoutPriority); bars->append(state); } rows->append(bars); } } } void ToolBarArea::restoreLayout(const YamlMappingPtr layout) { impl->restoreLayout(layout); } void ToolBarAreaImpl::restoreLayout(const YamlMappingPtr layout) { if(DEBUG_MODE){ cout << "ToolBarAreaImpl::restoreLayout()" << endl; } if(isBeforeDoingInitialLayout){ initialLayout = layout; return; } const YamlMappingPtr layoutOfToolBars = layout->findMapping("layoutOfToolBars"); if(!layoutOfToolBars->isValid()){ layoutToolBars(); return; } // make the map from name to toolBar typedef map ToolBarMap; ToolBarMap toolBarMap; for(size_t i=0; i < toolBarRows.size(); ++i){ ToolBarList& toolBars = toolBarRows[i]->toolBars; for(ToolBarList::iterator q = toolBars.begin(); q != toolBars.end(); ++q){ ToolBar* toolBar = *q; toolBarMap[toolBar->windowTitle()] = toolBar; } } toolBarRows.clear(); for(size_t i=0; i < newToolBars.size(); ++i){ toolBarMap[newToolBars[i]->windowTitle()] = newToolBars[i]; newToolBars[i]->setParent(self); } newToolBars.clear(); const YamlSequence* rows = layoutOfToolBars->get("rows").toSequence(); for(int i=0; i < rows->size(); ++i){ ToolBarRowPtr row(new ToolBarRow(self)); toolBarRows.push_back(row); const YamlSequence* bars = rows->get(i).toSequence(); for(int j=0; j < bars->size(); ++j){ const YamlMapping* state = bars->get(j).toMapping(); ToolBarMap::iterator it = toolBarMap.find(QString(state->get("name").toString().c_str())); if(it != toolBarMap.end()){ ToolBar* toolBar = it->second; toolBarMap.erase(it); row->toolBars.push_back(toolBar); toolBar->desiredX = state->get("x").toInt(); toolBar->layoutPriority = state->get("priority").toInt(); } } if(row->toolBars.empty()){ toolBarRows.pop_back(); } } for(ToolBarMap::iterator p = toolBarMap.begin(); p != toolBarMap.end(); ++p){ newToolBars.push_back(p->second); } layoutToolBars(); } void ToolBarArea::dragToolBar(ToolBar* toolBar, const QPoint& globalPos) { impl->dragToolBar(toolBar, globalPos); } void ToolBarAreaImpl::dragToolBar(ToolBar* toolBar, const QPoint& globalPos) { QPoint p = self->mapFromGlobal(globalPos); draggedToolBar = toolBar; draggedToolBarHasNotBeenInserted = true; draggedToolBar->desiredX = p.x(); dragY = p.y(); if(p.y() < 0){ toolBarRows.push_front(ToolBarRowPtr(new ToolBarRow(self))); } layoutToolBars(); draggedToolBar = 0; } void ToolBarAreaImpl::setNewToolBars() { vector numStrechablesOfRow; for(size_t i=0; i < newToolBars.size(); ++i){ setNewToolBar(newToolBars[i], numStrechablesOfRow); } newToolBars.clear(); for(size_t i=0; i < numStrechablesOfRow.size(); ++i){ int numStrechables = numStrechablesOfRow[i]; if(numStrechables > 0){ ToolBarRowPtr& row = toolBarRows[i]; ToolBar* lastToolBar = row->toolBars.back(); QRect r = lastToolBar->geometry(); int space = self->width() - (r.x() + r.width()); if(space > 0){ expandStrechableBars(row, (space / numStrechables), 0, 0, numStrechables, space); } } } } void ToolBarAreaImpl::expandStrechableBars (ToolBarRowPtr& row, int portion, int shift, int barIndex, int numStrechables, int lastSpace) { ToolBar* bar = row->toolBars[barIndex]; bar->desiredX += shift; if(bar->isStretchable()){ int addition = (numStrechables > 1) ? portion : lastSpace; shift += addition; lastSpace -= addition; --numStrechables; } ++barIndex; if(barIndex < (int)row->toolBars.size()){ expandStrechableBars(row, portion, shift, barIndex, numStrechables, lastSpace); } } void ToolBarAreaImpl::setNewToolBar(ToolBar* toolBar, vector& numStrechablesOfRow) { if(DEBUG_MODE){ cout << "ToolBarAreaImpl::setNewToolBar()" << endl; } if(toolBar){ ToolBarRowPtr toolBarRow; int rowIndex = -1; int width = toolBar->minimumSizeHint().width(); for(size_t i=0; i < toolBarRows.size(); ++i){ ToolBarRowPtr existingRow = toolBarRows[i]; ToolBar* lastToolBar = existingRow->toolBars.back(); if(lastToolBar){ QRect r = lastToolBar->geometry(); int lastX = r.x() + r.width(); int lastSpace = self->width() - lastX; if(width <= lastSpace){ toolBar->desiredX = lastX + 1; if(toolBar->isStretchable()){ width = std::min(toolBar->sizeHint().width(), lastSpace); } toolBarRow = existingRow; rowIndex = i; break; } } } if(!toolBarRow){ toolBar->desiredX = 0; rowIndex = toolBarRows.size(); toolBarRow.reset(new ToolBarRow(self)); toolBarRows.push_back(toolBarRow); numStrechablesOfRow.push_back(0); } toolBarRow->toolBars.push_back(toolBar); if(toolBar->isStretchable()){ numStrechablesOfRow[rowIndex]++; } toolBar->setParent(self); toolBar->setGeometry(toolBar->desiredX, 0, width, toolBar->sizeHint().height()); toolBar->show(); } } void ToolBarAreaImpl::layoutToolBars() { if(DEBUG_MODE){ cout << "ToolBarAreaImpl::layoutToolBars()" << endl; } if(!newToolBars.empty()){ setNewToolBars(); } if(DEBUG_MODE){ cout << "actually do ToolBarAreaImpl::layoutToolBars()" << endl; } areaHeight = 0; int rowTop = 0; spilledToolBarRow->toolBars.clear(); size_t i = 0; while(i < toolBarRows.size()){ layoutToolBarRow(toolBarRows[i], rowTop, false); if(!spilledToolBarRow->toolBars.empty()){ layoutToolBarRow(spilledToolBarRow, rowTop, false); } if(toolBarRows[i]->toolBars.empty()){ toolBarRows.erase(toolBarRows.begin() + i); } else { ++i; } } if(draggedToolBarHasNotBeenInserted){ ToolBarRowPtr row(new ToolBarRow(self)); toolBarRows.push_back(row); layoutToolBarRow(row, rowTop, true); } areaHeight = rowTop; if(areaHeight != prevAreaHeight){ self->setMinimumHeight(areaHeight); prevAreaHeight = areaHeight; } isBeforeDoingInitialLayout = false; } void ToolBarAreaImpl::layoutToolBarRow(ToolBarRowPtr toolBarRow, int& io_rowTop, bool isNewBottomRow) { int rowHeight = 0; bool draggedToolBarExists = false; ToolBarList& toolBars = toolBarRow->toolBars; // calculate the row height and remove the dragged tool bar if it is originally in this row ToolBarList::iterator p = toolBars.begin(); while(p != toolBars.end()){ ToolBar* toolBar = *p; if(toolBar == draggedToolBar){ draggedToolBarExists = true; p = toolBars.erase(p); } else { rowHeight = std::max(rowHeight, toolBar->sizeHint().height()); ++p; } } // Is the dragged tool bar moving to this row ? bool isDraggedToolBarMovingToThisRow = false; if(draggedToolBarHasNotBeenInserted){ int bottomBorder = 0; if(rowHeight == 0 && draggedToolBarExists){ int h = draggedToolBar->sizeHint().height(); bottomBorder = io_rowTop + h * 4 / 5; } else { bottomBorder = io_rowTop + rowHeight; } if(dragY < bottomBorder || isNewBottomRow){ isDraggedToolBarMovingToThisRow = true; rowHeight = std::max(rowHeight, draggedToolBar->sizeHint().height()); layoutToolBarRowWithDraggedToolBar(toolBars, io_rowTop, rowHeight); } } if(toolBars.empty()){ toolBarRow->separator.hide(); } else { if(!isDraggedToolBarMovingToThisRow){ layoutToolBarRowPart(toolBars, io_rowTop, rowHeight, 0, self->width()); } for(ToolBarList::iterator p = toolBars.begin(); p != toolBars.end(); ++p){ ToolBar* toolBar = *p; if(toolBar->height() != rowHeight){ toolBar->setGeometry(toolBar->x(), io_rowTop, toolBar->width(), rowHeight); } } io_rowTop += rowHeight; HSeparator& sep = toolBarRow->separator; int h = sep.sizeHint().height(); sep.setGeometry(0, io_rowTop, self->width(), h); sep.show(); io_rowTop += sep.sizeHint().height(); } } void ToolBarAreaImpl::layoutToolBarRowWithDraggedToolBar(ToolBarList& toolBars, int rowTop, int rowHeight) { int maxPriorty = normalizeLayoutPriorities(toolBars); draggedToolBar->layoutPriority = maxPriorty + 1; for(size_t i=0; i < toolBars.size(); ++i){ if(draggedToolBar->desiredX <= toolBars[i]->desiredX){ toolBars.insert(toolBars.begin() + i, draggedToolBar); draggedToolBarHasNotBeenInserted = false; break; } } if(draggedToolBarHasNotBeenInserted){ toolBars.push_back(draggedToolBar); draggedToolBarHasNotBeenInserted = false; } layoutToolBarRowPart(toolBars, rowTop, rowHeight, 0, self->width()); draggedToolBar->desiredX = draggedToolBar->geometry().x(); } /** @return The maximum priority in the normalized priorities */ int ToolBarAreaImpl::normalizeLayoutPriorities(ToolBarList& toolBars) { int maxPriority = 0; if(!toolBars.empty()){ ToolBarList sortedToolBars(toolBars); std::sort(sortedToolBars.begin(), sortedToolBars.end(), ToolBar::LayoutPriorityCmp()); int prevOldPriority = -1; int priority = -1; for(size_t i=0; i < sortedToolBars.size(); ++i){ ToolBar* bar = sortedToolBars[i]; if(bar->layoutPriority > prevOldPriority){ prevOldPriority = bar->layoutPriority; priority += 1; } bar->layoutPriority = priority; if(priority > maxPriority){ maxPriority = priority; } } } return maxPriority; } void ToolBarAreaImpl::layoutToolBarRowPart (ToolBarList& toolBars, int rowTop, int rowHeight, int partLeft, int partRight) { // find the index of the tool bar with the maximum priority int pivotIndex = 0; int maxPriority = -1; for(size_t i=0; i < toolBars.size(); ++i){ if(toolBars[i]->layoutPriority > maxPriority){ maxPriority = toolBars[i]->layoutPriority; pivotIndex = i; } } ToolBarList leftPartToolBars; int pivotAreaLeft = partLeft; for(int i=0; i < pivotIndex; ++i){ leftPartToolBars.push_back(toolBars[i]); pivotAreaLeft += toolBars[i]->minimumSizeHint().width(); } ToolBarList rightPartToolBars; int pivotAreaRight = partRight; for(size_t i = pivotIndex + 1; i < toolBars.size(); ++i){ rightPartToolBars.push_back(toolBars[i]); pivotAreaRight -= toolBars[i]->minimumSizeHint().width(); } ToolBar* pivotToolBar = toolBars[pivotIndex]; int x = pivotToolBar->desiredX; const QSize size = pivotToolBar->minimumSizeHint(); int width = size.width(); if(width < 0){ width = 0; } if(x + width > pivotAreaRight){ x = pivotAreaRight - width; } if(x < pivotAreaLeft){ x = pivotAreaLeft; } if(!leftPartToolBars.empty()){ layoutToolBarRowPart(leftPartToolBars, rowTop, rowHeight, partLeft, x); QRect r = leftPartToolBars.back()->geometry(); int leftPartRight = r.x() + r.width(); if(x <= leftPartRight){ x = leftPartRight + 1; } } int rightPartLeft = partRight; if(!rightPartToolBars.empty()){ layoutToolBarRowPart(rightPartToolBars, rowTop, rowHeight, x + width, partRight); rightPartLeft = rightPartToolBars.front()->geometry().x(); } if(pivotToolBar->isStretchable()){ width = pivotToolBar->maximumWidth(); int possibleWidth = rightPartLeft - x; if(width > possibleWidth){ width = possibleWidth; } } int height = (size.height() >= 0) ? size.height() : rowHeight; int y = rowTop + (rowHeight - height) / 2; pivotToolBar->setGeometry(x, y, width, height); pivotToolBar->show(); } choreonoid-1.1.0+dfsg/src/Base/ToolBarArea.h000066400000000000000000000022441207742442300205350ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_TOOL_BAR_AREA_H_INCLUDED #define CNOID_GUIBASE_TOOL_BAR_AREA_H_INCLUDED #include #include #include namespace cnoid { class ToolBar; class ToolBarAreaImpl; class YamlMapping; class ToolBarArea : public QWidget { public: ToolBarArea(QWidget* parent); ~ToolBarArea(); std::vector getAllToolBars(); void setInitialLayout(YamlMappingPtr layout); void storeLayout(YamlMappingPtr layout); void restoreLayout(const YamlMappingPtr layout); bool addToolBar(ToolBar* toolBar); void removeToolBar(ToolBar* toolBar); // called from ToolBar void dragToolBar(ToolBar* toolBar, const QPoint& globalPos); protected: virtual void resizeEvent(QResizeEvent* event); virtual bool event(QEvent* event); //virtual QSize sizeHint() const; //virtual QSize minimumSizeHint () const; private: ToolBarAreaImpl* impl; void doInitialLayout(); // called from MainWindowImpl; friend class MainWindowImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/TreeView.cpp000066400000000000000000000041041207742442300204640ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "TreeView.h" #include "ItemSelectionModel.h" using namespace cnoid; TreeView::TreeView(QWidget* parent) : QTreeView(parent) { connect(this, SIGNAL(collapsed(const QModelIndex&)), this, SLOT(onCollapsed(const QModelIndex&))); connect(this, SIGNAL(expanded(const QModelIndex&)), this, SLOT(onExpanded(const QModelIndex&))); connect(this, SIGNAL(activated(const QModelIndex&)), this, SLOT(onActivated(const QModelIndex&))); connect(this, SIGNAL(clicked(const QModelIndex&)), this, SLOT(onClicked(const QModelIndex&))); connect(this, SIGNAL(doubleClicked(const QModelIndex&)), this, SLOT(onDoubleClicked(const QModelIndex&))); connect(this, SIGNAL(entered(const QModelIndex&)), this, SLOT(onEntered(const QModelIndex&))); connect(this, SIGNAL(pressed(const QModelIndex&)), this, SLOT(onPressed(const QModelIndex&))); connect(this, SIGNAL(viewportEntered()), this, SLOT(onViewportEntered())); } void TreeView::setModel(QAbstractItemModel* model) { QItemSelectionModel* old = selectionModel(); QTreeView::setModel(model); if(old && !old->parent()){ delete old; } setSelectionModel(new ItemSelectionModel(model, this)); } ItemSelectionModel* TreeView::itemSelectionModel() const { return dynamic_cast(selectionModel()); } void TreeView::onCollapsed(const QModelIndex& index) { sigCollapsed_(index); } void TreeView::onExpanded(const QModelIndex& index) { sigExpanded_(index); } void TreeView::onActivated(const QModelIndex& index) { sigActivated_(index); } void TreeView::onClicked(const QModelIndex& index) { sigClicked_(index); } void TreeView::onDoubleClicked(const QModelIndex& index) { sigDoubleClicked_(index); } void TreeView::onEntered(const QModelIndex& index) { sigEntered_(index); } void TreeView::onPressed(const QModelIndex& index) { sigPressed_(index); } void TreeView::onViewportEntered(void) { sigViewportEntered_(); } choreonoid-1.1.0+dfsg/src/Base/TreeView.h000066400000000000000000000050131207742442300201310ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_TREE_VIEW_H_INCLUDED #define CNOID_GUIBASE_TREE_VIEW_H_INCLUDED #include "ItemSelectionModel.h" #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT TreeView : public QTreeView { Q_OBJECT public: TreeView(QWidget* parent = 0); virtual void setModel(QAbstractItemModel* model); ItemSelectionModel* itemSelectionModel() const; inline SignalProxy< boost::signal > sigCollapsed() { return sigCollapsed_; } inline SignalProxy< boost::signal > sigExpanded() { return sigExpanded_; } inline SignalProxy< boost::signal > sigActivated() { return sigActivated_; } inline SignalProxy< boost::signal > sigClicked() { return sigClicked_; } inline SignalProxy< boost::signal > sigDoubleClicked() { return sigDoubleClicked_; } inline SignalProxy< boost::signal > sigEntered() { return sigEntered_; } inline SignalProxy< boost::signal > sigPressed() { return sigPressed_; } inline SignalProxy< boost::signal > sigViewportEntered() { return sigViewportEntered_; } private Q_SLOTS: void onCollapsed(const QModelIndex& index); void onExpanded(const QModelIndex& index); void onActivated(const QModelIndex& index); void onClicked(const QModelIndex& index); void onDoubleClicked(const QModelIndex& index); void onEntered(const QModelIndex& index); void onPressed(const QModelIndex& index); void onViewportEntered(void); private: boost::signal sigCollapsed_; boost::signal sigExpanded_; boost::signal sigActivated_; boost::signal sigClicked_; boost::signal sigDoubleClicked_; boost::signal sigEntered_; boost::signal sigPressed_; boost::signal sigViewportEntered_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/TreeWidget.cpp000066400000000000000000000047171207742442300210070ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "TreeWidget.h" using namespace cnoid; TreeWidget::TreeWidget(QWidget* parent) : QTreeWidget(parent) { connect(this, SIGNAL(currentItemChanged(QTreeWidgetItem*, QTreeWidgetItem*)), this, SLOT(onCurrentItemChanged(QTreeWidgetItem*, QTreeWidgetItem*))); connect(this, SIGNAL(itemActivated(QTreeWidgetItem*, int)), this, SLOT(onItemActivated(QTreeWidgetItem*, int))); connect(this, SIGNAL(itemChanged(QTreeWidgetItem*, int)), this, SLOT(onItemChanged(QTreeWidgetItem*, int))); connect(this, SIGNAL(itemClicked(QTreeWidgetItem*, int)), this, SLOT(onItemClicked(QTreeWidgetItem*, int))); connect(this, SIGNAL(itemCollapsed(QTreeWidgetItem*)), this, SLOT(onItemCollapsed(QTreeWidgetItem*))); connect(this, SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)), this, SLOT(onItemDoubleClicked(QTreeWidgetItem*, int))); connect(this, SIGNAL(itemEntered(QTreeWidgetItem*, int)), this, SLOT(onItemEntered(QTreeWidgetItem*, int))); connect(this, SIGNAL(itemExpanded(QTreeWidgetItem*)), this, SLOT(onItemExpanded(QTreeWidgetItem*))); connect(this, SIGNAL(itemPressed(QTreeWidgetItem*, int)), this, SLOT(onItemPressed(QTreeWidgetItem*, int))); connect(this, SIGNAL(itemSelectionChanged()), this, SLOT(onItemSelectionChanged())); } void TreeWidget::onCurrentItemChanged(QTreeWidgetItem* current, QTreeWidgetItem* previous) { sigCurrentItemChanged_(current, previous); } void TreeWidget::onItemActivated(QTreeWidgetItem* item, int column) { sigItemActivated_(item, column); } void TreeWidget::onItemChanged(QTreeWidgetItem* item, int column) { sigItemChanged_(item, column); } void TreeWidget::onItemClicked(QTreeWidgetItem* item, int column) { sigItemClicked_(item, column); } void TreeWidget::onItemCollapsed(QTreeWidgetItem* item) { sigItemCollapsed_(item); } void TreeWidget::onItemDoubleClicked(QTreeWidgetItem* item, int column) { sigItemDoubleClicked_(item, column); } void TreeWidget::onItemEntered(QTreeWidgetItem* item, int column) { sigItemEntered_(item, column); } void TreeWidget::onItemExpanded(QTreeWidgetItem* item) { sigItemExpanded_(item); } void TreeWidget::onItemPressed(QTreeWidgetItem* item, int column) { sigItemPressed_(item, column); } void TreeWidget::onItemSelectionChanged(void) { sigItemSelectionChanged_(); } choreonoid-1.1.0+dfsg/src/Base/TreeWidget.h000066400000000000000000000064171207742442300204530ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_TREE_WIDGET_H_INCLUDED #define CNOID_GUIBASE_TREE_WIDGET_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT TreeWidget : public QTreeWidget { Q_OBJECT public: TreeWidget(QWidget* parent = 0); inline SignalProxy< boost::signal > sigCurrentItemChanged() { return sigCurrentItemChanged_; } inline SignalProxy< boost::signal > sigItemActivated() { return sigItemActivated_; } inline SignalProxy< boost::signal > sigItemChanged() { return sigItemChanged_; } inline SignalProxy< boost::signal > sigItemClicked() { return sigItemClicked_; } inline SignalProxy< boost::signal > sigItemCollapsed() { return sigItemCollapsed_; } inline SignalProxy< boost::signal > sigItemDoubleClicked() { return sigItemDoubleClicked_; } inline SignalProxy< boost::signal > sigItemEntered() { return sigItemEntered_; } inline SignalProxy< boost::signal > sigItemExpanded() { return sigItemExpanded_; } inline SignalProxy< boost::signal > sigItemPressed() { return sigItemPressed_; } inline SignalProxy< boost::signal > sigItemSelectionChanged() { return sigItemSelectionChanged_; } private Q_SLOTS: void onCurrentItemChanged(QTreeWidgetItem* current, QTreeWidgetItem* previous); void onItemActivated(QTreeWidgetItem* item, int column); void onItemChanged(QTreeWidgetItem* item, int column); void onItemClicked(QTreeWidgetItem* item, int column); void onItemCollapsed(QTreeWidgetItem* item); void onItemDoubleClicked(QTreeWidgetItem* item, int column); void onItemEntered(QTreeWidgetItem* item, int column); void onItemExpanded(QTreeWidgetItem* item); void onItemPressed(QTreeWidgetItem* item, int column); void onItemSelectionChanged(void); private: boost::signal sigCurrentItemChanged_; boost::signal sigItemActivated_; boost::signal sigItemChanged_; boost::signal sigItemClicked_; boost::signal sigItemCollapsed_; boost::signal sigItemDoubleClicked_; boost::signal sigItemEntered_; boost::signal sigItemExpanded_; boost::signal sigItemPressed_; boost::signal sigItemSelectionChanged_; }; } #endif choreonoid-1.1.0+dfsg/src/Base/Vector3SeqItem.cpp000066400000000000000000000036351207742442300215570ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "Vector3SeqItem.h" #include "ItemManager.h" #include "Archive.h" #include "gettext.h" using namespace cnoid; void Vector3SeqItem::initialize(ExtensionManager* ext) { ext->itemManager().registerClass(N_("Vector3SeqItem")); } Vector3SeqItem::Vector3SeqItem() : seq_(new Vector3Seq()) { } Vector3SeqItem::Vector3SeqItem(Vector3SeqPtr seq) : seq_(seq) { } Vector3SeqItem::Vector3SeqItem(const Vector3SeqItem& org) : Item(org), seq_(new Vector3Seq(*org.seq_)) { } Vector3SeqItem::~Vector3SeqItem() { } bool Vector3SeqItem::loadPlainFormat(const std::string& filename) { bool loaded = seq_->loadPlainFormat(filename); notifyUpdate(); return loaded; } bool Vector3SeqItem::saveAsPlainFormat(const std::string& filename) { return seq_->saveAsPlainFormat(filename); } ItemPtr Vector3SeqItem::doDuplicate() const { return new Vector3SeqItem(*this); } void Vector3SeqItem::notifyUpdate() { setInconsistencyWithLastAccessedFile(); Item::notifyUpdate(); } bool Vector3SeqItem::store(Archive& archive) { if(overwrite()){ archive.writeRelocatablePath("filename", lastAccessedFileName()); archive.write("format", lastAccessedFileFormatId()); return true; } return false; } bool Vector3SeqItem::restore(const Archive& archive) { std::string filename, formatId; if(archive.readRelocatablePath("filename", filename) && archive.read("format", formatId)){ if(load(filename, formatId)){ return true; } } return false; } void Vector3SeqItem::doPutProperties(PutPropertyFunction& putProperty) { putProperty(_("Frame rate"), seq_->frameRate()); putProperty(_("Number of frames"), seq_->numFrames()); putProperty(_("Time length"), seq_->numFrames() / seq_->frameRate()); putProperty(_("Time step"), 1.0 / seq_->frameRate()); } choreonoid-1.1.0+dfsg/src/Base/Vector3SeqItem.h000066400000000000000000000022141207742442300212140ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_VECTOR3_SEQ_ITEM_H_INCLUDED #define CNOID_GUIBASE_VECTOR3_SEQ_ITEM_H_INCLUDED #include "Item.h" #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT Vector3SeqItem : public Item { public: static void initialize(ExtensionManager* ext); Vector3SeqItem(); Vector3SeqItem(Vector3SeqPtr seq); inline const Vector3SeqPtr& seq() { return seq_; } bool loadPlainFormat(const std::string& filename); bool saveAsPlainFormat(const std::string& filename); virtual void notifyUpdate(); protected: Vector3SeqItem(const Vector3SeqItem& org); virtual ItemPtr doDuplicate() const; virtual void doPutProperties(PutPropertyFunction& putProperty); virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); private: virtual ~Vector3SeqItem(); Vector3SeqPtr seq_; }; typedef boost::intrusive_ptr Vector3SeqItemPtr; } #endif choreonoid-1.1.0+dfsg/src/Base/View.cpp000066400000000000000000000036311207742442300176500ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "View.h" #include "MainWindow.h" #include #include using namespace std; using namespace cnoid; View::View() { //setSizePolicy(QSizePolicy(QSizePolicy::Ignored, QSizePolicy::Ignored)); isActive_ = false; isManagedByMainWindow = false; defaultLayoutArea_ = CENTER; } View::~View() { if(isActive_){ onDeactivated(); } if(isManagedByMainWindow){ MainWindow::instance()->removeView(this); } } void View::setName(const QString& name) { setObjectName(name); setWindowTitle(name); } bool View::isActive() const { return isActive_; } void View::showEvent(QShowEvent* event) { if(!isActive_){ isActive_ = true; onActivated(); sigActivated_(); } } void View::hideEvent(QHideEvent* event) { if(isActive_){ isActive_ = false; onDeactivated(); sigDeactivated_(); } } /** Virtual function which is called when the view becomes visible on the main window. @note In the current implementation, this function may be continuously called two or three times when the perspective changes, and the number of calles does not necessarily corresponds to the number of 'onDeactivated()' calles. @todo improve the behavior written as note */ void View::onActivated() { } void View::onDeactivated() { } void View::setDefaultLayoutArea(LayoutArea area) { defaultLayoutArea_ = area; } View::LayoutArea View::defaultLayoutArea() const { return defaultLayoutArea_; } void View::setLayout(QLayout* layout) { const int margin = 0; layout->setContentsMargins(margin, margin, margin, margin); QWidget::setLayout(layout); } QWidget* View::indicatorOnInfoBar() { return 0; } bool View::storeState(Archive& archive) { return true; } bool View::restoreState(const Archive& archive) { return true; } choreonoid-1.1.0+dfsg/src/Base/View.h000066400000000000000000000035651207742442300173230ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_GUIBASE_VIEW_H_INCLUDED #define CNOID_GUIBASE_VIEW_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class ToolBar; class Archive; class ExtensionManager; class CNOID_EXPORT View : public QWidget { public: View(); virtual ~View(); void setName(const QString& name); inline QString name() const { return objectName(); } enum LayoutArea { LEFT = 0, LEFT_TOP = 0, LEFT_BOTTOM = 1, CENTER = 2, RIGHT = 3, BOTTOM = 4, NUM_AREAS }; void setDefaultLayoutArea(LayoutArea area); LayoutArea defaultLayoutArea() const; bool isActive() const; inline SignalProxy< boost::signal > sigActivated() { return sigActivated_; } inline SignalProxy< boost::signal > sigDeactivated() { return sigDeactivated_; } void setLayout(QLayout* layout); virtual QWidget* indicatorOnInfoBar(); protected: virtual void onActivated(); virtual void onDeactivated(); virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); private: // Qt events (make hidden) virtual void showEvent(QShowEvent* event); virtual void hideEvent(QHideEvent* event); LayoutArea defaultLayoutArea_; bool isManagedByMainWindow; bool isActive_; boost::signal sigActivated_; boost::signal sigDeactivated_; friend class MainWindow; friend class MainWindowImpl; friend class ProjectManagerImpl; }; } #endif choreonoid-1.1.0+dfsg/src/Base/VrmlToOsgConverter.cpp000066400000000000000000000335161207742442300225270ustar00rootroot00000000000000 /** @author Shin'ichiro Nakaoka */ #include "VrmlToOsgConverter.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool AVOID_SPECULAR_BUG_OF_OPENGL_DRIVER = true; } namespace cnoid { class VrmlToOsgConverterImpl { public: VrmlToOsgConverterImpl(); osg::Node* convert(VrmlNode* vrmlNode); osg::Node* convertNode(VrmlNode* vnode); osg::Node* convertGroupNode(VrmlGroup* vgroup); osg::Group* createTransformNode(VrmlTransform* vt); osg::Node* convertShapeNode(VrmlNode* vnode); osg::Material* createMaterial(VrmlMaterial* vm); osg::Geometry* createGeometryFromIndexedFaceSet(VrmlIndexedFaceSet* vface, float alpha); VrmlMaterialPtr defaultMaterial; osg::ref_ptr stateSetForTransformWithScaling; osg::ref_ptr stateSetForTransformWithUniformScaling; typedef map VrmlNodeToOsgMaterialMap; VrmlNodeToOsgMaterialMap vrmlNodeToOsgMaterialMap; typedef map VrmlNodeToOsgNodeMap; VrmlNodeToOsgNodeMap vrmlNodeToOsgNodeMap; typedef map VrmlGeometryToOsgGeometryMap; VrmlGeometryToOsgGeometryMap vrmlGeometryToOsgGeometryMap; osgUtil::Optimizer optimizer; }; } VrmlToOsgConverter::VrmlToOsgConverter() { impl = new VrmlToOsgConverterImpl(); } VrmlToOsgConverterImpl::VrmlToOsgConverterImpl() { defaultMaterial = new VrmlMaterial(); stateSetForTransformWithScaling = new osg::StateSet(); stateSetForTransformWithScaling->setMode(GL_NORMALIZE, osg::StateAttribute::ON); stateSetForTransformWithUniformScaling = new osg::StateSet(); stateSetForTransformWithUniformScaling->setMode(GL_RESCALE_NORMAL, osg::StateAttribute::ON); } VrmlToOsgConverter::~VrmlToOsgConverter() { delete impl; } osg::Node* VrmlToOsgConverter::convert(VrmlNodePtr vrmlNode) { return impl->convert(vrmlNode.get()); } osg::Node* VrmlToOsgConverterImpl::convert(VrmlNode* vrmlNode) { osg::Node* node = convertNode(vrmlNode); if(node){ // needed ? osg::StateSet* state = node->getOrCreateStateSet(); osg::LightModel* lightModel = new osg::LightModel; lightModel->setTwoSided(true); state->setAttributeAndModes(lightModel); optimizer.optimize(node); } return node; } osg::Node* VrmlToOsgConverterImpl::convertNode(VrmlNode* vnode) { osg::Node* node = 0; VrmlNodeToOsgNodeMap::iterator p = vrmlNodeToOsgNodeMap.find(vnode); if(p != vrmlNodeToOsgNodeMap.end()){ node = p->second; } else { if(VrmlProtoInstance* protoInstance = dynamic_cast(vnode)){ vnode = protoInstance->actualNode.get(); } if(vnode){ if(VrmlGroup* group = dynamic_cast(vnode)){ node = convertGroupNode(group); } else if(VrmlShape* shape = dynamic_cast(vnode)){ node = convertShapeNode(shape); } if(node){ node->setName(vnode->defName); vrmlNodeToOsgNodeMap.insert(make_pair(vnode, node)); } } } return node; } osg::Node* VrmlToOsgConverterImpl::convertGroupNode(VrmlGroup* vgroup) { osg::Group* group; if(VrmlTransform* transform = dynamic_cast(vgroup)){ group = createTransformNode(transform); } else { group = new osg::Group; } int num = vgroup->children.size(); for(int i=0; i < num; i++){ osg::Node* child = convertNode(vgroup->children[i].get()); if(child){ group->addChild(child); } } if(group->getNumChildren() == 0){ group->ref(); group->unref(); group = 0; } return group; } osg::Group* VrmlToOsgConverterImpl::createTransformNode(VrmlTransform* vt) { osg::MatrixTransform* transform = new osg::MatrixTransform; const SFRotation::Vector3& a = vt->rotation.axis(); osg::Matrix R = osg::Matrix::rotate(vt->rotation.angle(), osg::Vec3d(a[0], a[1], a[2])); osg::Matrix T; T.setTrans(vt->translation[0], vt->translation[1], vt->translation[2]); osg::Matrix C; C.setTrans(vt->center[0], vt->center[1], vt->center[2]); osg::Matrix Cinv; Cinv.invert(C); const SFRotation::Vector3& sa = vt->scaleOrientation.axis(); osg::Vec3d scaleAxis(sa[0], sa[1], sa[2]); osg::Matrix SR = osg::Matrix::rotate(vt->scaleOrientation.angle(), scaleAxis); osg::Matrix SRinv; SRinv.invert(SR); const SFVec3f& s = vt->scale; osg::Matrix S = osg::Matrix::scale(s[0], s[1], s[2]); T.preMult(C); T.preMult(R); T.preMult(SR); T.preMult(S); T.preMult(SRinv); T.preMult(Cinv); transform->setMatrix(T); if((s.array() != 1.0).any()){ if(s[0] == s[1] && s[1] == s[2]){ transform->setStateSet(stateSetForTransformWithUniformScaling.get()); } else { transform->setStateSet(stateSetForTransformWithScaling.get()); } } return transform; } osg::Node* VrmlToOsgConverterImpl::convertShapeNode(VrmlNode* vnode) { VrmlShape* vrmlShape = static_cast(vnode); VrmlMaterial* vm = defaultMaterial.get(); if(vrmlShape->appearance && vrmlShape->appearance->material){ vm = vrmlShape->appearance->material.get(); } float alpha = 1.0 - vm->transparency; osg::Geode* geode = 0; osg::Geometry* geometry = 0; VrmlGeometry* vrmlGeometry = dynamic_node_cast(vrmlShape->geometry).get(); if(vrmlGeometry){ VrmlGeometryToOsgGeometryMap::iterator p = vrmlGeometryToOsgGeometryMap.find(vrmlGeometry); if(p != vrmlGeometryToOsgGeometryMap.end()){ geometry = p->second; } else { if(VrmlIndexedFaceSet* faceSet = dynamic_cast(vrmlGeometry)){ geometry = createGeometryFromIndexedFaceSet(faceSet, alpha); } if(geometry){ vrmlGeometryToOsgGeometryMap.insert(make_pair(vrmlGeometry, geometry)); } } } if(geometry){ geode = new osg::Geode; geode->addDrawable(geometry); osg::Material* material; VrmlNodeToOsgMaterialMap::iterator p = vrmlNodeToOsgMaterialMap.find(vm); if(p != vrmlNodeToOsgMaterialMap.end()){ material = p->second; } else { material = createMaterial(vm); vrmlNodeToOsgMaterialMap.insert(make_pair(vm, material)); } osg::StateSet* stateSet = geode->getOrCreateStateSet(); stateSet->setAttributeAndModes(material); if(alpha < 1.0f){ // Enable blending, select transparent bin. stateSet->setMode(GL_BLEND, osg::StateAttribute::ON); stateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); // Enable depth test so that an opaque polygon will occlude a transparent one behind it. stateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON); // Conversely, disable writing to depth buffer so that // a transparent polygon will allow polygons behind it to shine thru. // OSG renders transparent polygons after opaque ones. osg::Depth* depth = new osg::Depth; depth->setWriteMask(false); stateSet->setAttributeAndModes(depth, osg::StateAttribute::ON); // Disable conflicting modes. //stateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF); } } return geode; } osg::Material* VrmlToOsgConverterImpl::createMaterial(VrmlMaterial* vm) { osg::Material* material = new osg::Material(); float alpha = 1.0 - vm->transparency; osg::Vec4 diffuse(vm->diffuseColor[0], vm->diffuseColor[1], vm->diffuseColor[2], alpha); material->setDiffuse(osg::Material::FRONT_AND_BACK, diffuse); float a = vm->ambientIntensity; osg::Vec4 ambient(diffuse[0] * a, diffuse[1] * a, diffuse[2] * a, alpha); material->setAmbient(osg::Material::FRONT_AND_BACK, ambient); osg::Vec4 emission(vm->emissiveColor[0], vm->emissiveColor[1], vm->emissiveColor[2], alpha); material->setEmission(osg::Material::FRONT_AND_BACK, emission); osg::Material::Face specularFace = AVOID_SPECULAR_BUG_OF_OPENGL_DRIVER ? osg::Material::FRONT : osg::Material::FRONT_AND_BACK; osg::Vec4 specular(vm->specularColor[0], vm->specularColor[1], vm->specularColor[2], 1.0); material->setSpecular(specularFace, specular); float shininess = (127.0 * vm->shininess) + 1.0; material->setShininess(specularFace, shininess); return material; } osg::Geometry* VrmlToOsgConverterImpl::createGeometryFromIndexedFaceSet(VrmlIndexedFaceSet* vface, float alpha) { osg::Geometry* geometry = new osg::Geometry; if(!vface->ccw){ osg::StateSet* stateSet = geometry->getOrCreateStateSet(); stateSet->setAttributeAndModes(new osg::FrontFace(osg::FrontFace::CLOCKWISE)); } if(!vface->solid){ } if(vface->coord){ osg::Vec3Array* vertices = new osg::Vec3Array; MFVec3f& point = vface->coord->point; int size = point.size(); for(int i=0; i < size; i++){ vertices->push_back(osg::Vec3(point[i][0], point[i][1], point[i][2])); } geometry->setVertexArray(vertices); } geometry->addPrimitiveSet(new osg::DrawArrayLengths(osg::PrimitiveSet::POLYGON)); osg::IntArray* vertexIndices = new osg::IntArray(); int numVertex = 0; int numIndices = vface->coordIndex.size(); for(int i=0; i < numIndices; i++) { int index = vface->coordIndex[i]; if (index < 0) { ((osg::DrawArrayLengths*)geometry->getPrimitiveSet(0))->push_back(numVertex); numVertex = 0; } else { vertexIndices->push_back(index); numVertex++; } } geometry->setVertexIndices(vertexIndices); /* old code unsigned int i = 0; unsigned int begin = 0; int numIndices = vface->coordIndex.size(); while(i < numIndices){ int index = vface->coordIndex[i]; if(index < 0){ int numVertices = i - begin; if(numVertices >= 3){ osg::PrimitiveSet::Mode pmode; switch(numVertices){ case 3: pmode = osg::PrimitiveSet::TRIANGLES; break; case 4: pmode = osg::PrimitiveSet::QUADS; break; default: pmode = osg::PrimitiveSet::POLYGON; break; } osg::DrawElementsUShort* drawElements = new osg::DrawElementsUShort(pmode); for(unsigned int j=begin; j < i; ++j){ drawElements->push_back(vface->coordIndex[j]); } geometry->addPrimitiveSet(drawElements); } begin = i + 1; } i++; } */ if(vface->normal){ osg::Vec3Array* normals = new osg::Vec3Array; MFVec3f& vec = vface->normal->vector; int size = vec.size(); for(int i=0; i < size; i++){ normals->push_back(osg::Vec3(vec[i][0], vec[i][1], vec[i][2])); } geometry->setNormalArray(normals); if(vface->normalIndex.empty()){ geometry->setNormalIndices(geometry->getVertexIndices()); } else { int size = vface->normalIndex.size(); osg::UIntArray* indices = new osg::UIntArray; for(int i=0; i < size; ++i){ int index = vface->normalIndex[i]; if(index >= 0){ indices->push_back(index); } } geometry->setNormalIndices(indices); } if(vface->normalPerVertex == true){ geometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX); } else { geometry->setNormalBinding(osg::Geometry::BIND_PER_PRIMITIVE); } } if(vface->color){ MFColor& c = vface->color->color; if(!c.empty()){ osg::Vec4Array* colors = new osg::Vec4Array; int size = c.size(); for(int i=0; i < size; i++){ colors->push_back(osg::Vec4(c[i][0], c[i][1], c[i][2], alpha)); } geometry->setColorArray(colors); if(vface->colorIndex.empty()){ geometry->setColorIndices(geometry->getVertexIndices()); } else { int size = vface->colorIndex.size(); osg::UIntArray* indices = new osg::UIntArray; // osg::TemplateIndexArray *indices; // indices = new osg::TemplateIndexArray; for(int i=0; i < size; i++){ int index = vface->colorIndex[i]; if(index >= 0){ indices->push_back(index); } } geometry->setColorIndices(indices); } if(vface->colorPerVertex == true){ geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX); } else { geometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE); } } } if(!vface->normal){ osgUtil::SmoothingVisitor::smooth(*geometry); } return geometry; } choreonoid-1.1.0+dfsg/src/Base/VrmlToOsgConverter.h000066400000000000000000000007731207742442300221730ustar00rootroot00000000000000 #ifndef CNOID_GUIBASE_VRML_TO_OSG_CONVERTER_H_INCLUDED #define CNOID_GUIBASE_VRML_TO_OSG_CONVERTER_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class VrmlToOsgConverterImpl; class CNOID_EXPORT VrmlToOsgConverter { public: VrmlToOsgConverter(); ~VrmlToOsgConverter(); osg::Node* convert(VrmlNodePtr vrmlNode); private: VrmlToOsgConverterImpl* impl; }; }; #endif choreonoid-1.1.0+dfsg/src/Base/exportdecl.h000066400000000000000000000004461207742442300205550ustar00rootroot00000000000000 #ifdef CNOID_EXPORT #undef CNOID_EXPORT #endif #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) # ifdef CnoidBase_EXPORTS # define CNOID_EXPORT __declspec(dllexport) # else # define CNOID_EXPORT __declspec(dllimport) # endif #else # define CNOID_EXPORT #endif choreonoid-1.1.0+dfsg/src/Base/icons/000077500000000000000000000000001207742442300173425ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Base/icons/accgraph.png000066400000000000000000000033331207742442300216220ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<XIDATH‰µ–ml“×Ç÷>ýر'$Ø ‰!@’’°vÀ¥{Q7ÕO:耮 ¤Uða0ªµV˜ØŠ¢­UK§hT(”ä ¶ h‹ Œ&1 M!¼$äe81qìØ~ž»uXúa{>]ýÎýÿï9÷¡”âë4}Ì1M³ôg~îò@àŽÙÒâUJ%›ššîš¦Y ‡;Çû¿\º´Ü§ëú«\ …BÅBg8î …BµBO8þ@„B!À3……óW–”ìÑ„°ÞïéYÿÎíÛmš¦Ýµm»@JÙ9æ¿UU¥û ã `]~zs[›.„pJ)o¦ÓéZ)e–”òÓ1U^oöóÅÅՅȨ+*Ú÷Þ½{?Û¶OJÈtK)ý^oî7ÜîÝÜ3½ÞãRÊ_ !€WÓ´ Â"€J)ضíàí´Rmº(ïˆÅVN«¯ß?&‹ÞÔÓ<žuÓ&Lø‰•JU¸ cðѲ2ߟïÞ{âÄGƒx±,ÊÉùxtáÂÃ_‘èHMÍß5!žèŠÇ·†Ó%å–éô‘ç/]Z!ûú*ôË—‘72²ê)È*\@h€É@°+++_´èM)e3€^áõÎÕ„X`+•z­««ý»¹¹žg'MB¢ÚÑÖVçº~ý­gl;?\ö VP`õ $–Ù¶gð팆a@7¤”¥À0 õÓ§;™RêŸ7GFμ1cFx»½³³$¯£cÏËà)'²³ÿ]¿fÍÔ¼ìì“O75mhlmýl—Rn\„”]\üžeY;vì<€Þ;:j·ãñN`†ÙÒ2°57÷þ¾&þô"à8pØáødûêÕÝyÙÙSïŽv ¿?0,eTX– t8.X>Ÿ_X–Ó4ÍA@êùNg@_2ypvt¤vwuål¶mŠ€.`¿”ýV0¸:•õ€¥ÔYâñèË*k¤VM;–©ÐÝ!D5 š#‘ÙÖ¶g)8'gw£yyë¯]k··nPìr4ZZ~T ’ñ ‰C@¶Rʇût܃©Tät$²^ëé)¬„/Ô´û|w“óæý`í²eßßYQ1!fYñç>ÿü%§”æw2¸(0àó% üJ Q išÞ¼aÃN§”}Õ¥KƒYY>¥TZkoŸkò?; ¤ËÊŽ+¥î>ž›; ’JõhÓ"‘o>’Á] “÷à¦mÛi¥”>+;û‡nMËÕ÷76Ö,çOŽF7ÏÎE€xA*(-}µ;‘H¯–óe«žvœ;7ý9cú´OVVêkƒÁ7êZ[gO6Œ|·¦­âºišçÐÐœJbÜ­¾5s¦pÎh¸}ûzZ©ÙºyæþýˆûƒBØöjn‡ìââ“^±"4É0V1µíúŸ^¼ø; …Ïœ•(¥F…ÿ ”RÉY99Éß”•íôézù…ÁÁÛ¯_ÿ"C¬{Ü;ßQJMõxò¶——¿ìÑ´Y·âñÖMííë†Òiû+ôcþ‹Ë—ÏXî÷W>öúëïÿ¿àáЯ­í¾6<üÛ->zô4€øº×–ÿ#a²›¤½0|IEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/collisionlines.png000066400000000000000000000025721207742442300231040ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<÷IDATH‰U[lUþÎ\vf¯ÝÝnKË¥[Zvhµµµ%€"XM,„úˆÄ^¼Ä;ðæ¥hPMK xÁ˜/€D¹¥µdŠ)ô†eÛÝÙeiwgwvæøŠk-ž?Nrþó}_ξä¥SM<^T¹÷<†Áƒ­ ¤È"Kdª€`¤Š°„P˜©NdÉõŸ¤Îó6³Øón&ÎÉÖyŸt¦Tþ7Oª€€ËD– &@”´Q•¼Ã?ák'²$N¸l2å…Oç B¶eááZßJF$g79KµoRª¨p^f_Öcö¥B.ßAdiYb-"o/”g€ãØš»÷ÅE9¯8ˬ{-¹ü-3H·ÜÏ—ñÒ#æÖxG*TûyE¹s–ý´¥Ê¿@iv±—L—òÁ6ȱ/ÉßW믴i–›Æèaª¨ƒS Š1n™ŸÆN%ÆÛVäM dâîÇ渼ðÂåsòv¯íø‚Æ9MËwÔ¹{š‚ÉýÕL\ÿê¢TTÿ¨gßµQ—™…Í-M¾úÆG–Í)±úàÃÜÚ²¬g_ªfŦŎ¯ƒšK·REdâ!”ÒŒD–Ö®›¿2°sÃö5’ˆÁ€nêÐ ‘TÛV}2¹~3ÀÀßIËÝ#Õž7 O6±<ÃqV6Í2ñã›í!VQDzÌàM€[tÃX°ÐcÃ…Ó¿Ã¢[¸Dy&g ͈víªî‚ˆ^‚€ÇA<ÇIìÑoÉ/y~¦/°´Ô2Í– ï=!A‚ .ðà‘@QD¡BEa„¨ÂoÇ» -—FáäÅÔpú}ª¨'&´ˆÈ’wq/Ëz‘×·¨±Æÿx]5“ËçÀ¼ðÂÛâÞÑkhß4ÖÙÞ}SOéû¡d3UÔà]Nî>÷„4h&²ôÈà¹áŸ¼»${N&L¤‘† ƈÇøxý‰Ñ!õ ¡›©¢Sr‘¥‡ßvðÅÖµÖÒš"hÐ0ŽqDEaÄC °õ«Öé¹ VðÆ”\DdIvä[¿mؽ,¿¸¨ñž$‚]asuãrÆ +tè8uò,MrñWIˆ’(¾Ù~,zíÇ¡ïµPjUÔtÆ1ó½õ¶Bñ`msenïé~íÈÖ‘„ªõÏ›URßX•Ó74ˆðu¥ÕEèHÞù]§˜%äfWH¶™ 9>Rl®ºÒ2ô ‘¥eTQ£˜é"œ—?¹¹³Ç7w¥né{ÔûÊ݇"ˆ eûðh_Øx­ý%¿³ÔÆ .Ëùèý[ˆ,Uuo¤ ]E Ëd‘"KµTQ¯2@*¥4…VÆAÚbçã ãG‡Ë“¿?£Š:&º…ùö«½«£›þy%x&O~pdϱq¡˜eèÃwÌÑ•8~ãeíh°@1ªÁÑ6xhÓmçÜ…,K¦‘g_–wxõå*5Ü@À“€‡µ/É»Ü8²–Ú—æõOidRE5¨¢¦2¹À0ŒÙý†Rñ¨¶Ÿ*êuª¨F<¤­?±ã\ˆq ‘%k&ܤ6½w™Ô”.}Ù?¢$ßúû¬+ü³Ú;“N¥³”O†ç&+Yòð³YÌFª¨ñ{kÉú"Âa.€Žÿû‚2âÀEã¦Ùz*j8é68i`2‚¿Êön°[ï†zIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/floorgrid.png000066400000000000000000000024161207742442300220420ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<‹IDATH‰½•kˆÔeÆçÿfgfÝuöæeÖKëz'Úd+¿h»’”BW¨@"Â(# Eü©ß4òCXTŸÂH„"¡ÈÄ SDÓ]gg/îèÌîÎÌÎþ/ïéÃÚæ²vù 8¼pÞ—ó<ÏûÀ9¢ªÜÍ0wµûÿàüíE:¦Å ;kôŽèÈ᧠.ur ýàf0‰.ÌôN‰­èDâE |3+AvÖ$Cåv&;éL•ï9‰©É™~™°w¦W!©óh©-5¢^qn€›C¢9¤j"Y$Ú-¦îŠ$_<dgõÞV–¿|Rm~±˜äIìp;\ƒI3ÉW`Re-\­a¶EªùŠðjz?®Åf n‚°¸\µc‘†ý¯ÃÞƒ“8éŒk‡?<)¯ ‰ØÁ: ³íÀxÕ€‡ZEœ<¦î¬Hòš†Ù%H¼O’Ï~&΂¼-¼ùDOØÒ¡“hùУØÁ…2ååm˜š2ÁÅ”´™Ú]o!Q «Ú÷m%2í”àÕÐáfìy:´{•jF0õ—&™ì¤3 :·#U:òõ Lj€àr+¦¾‰z€Å™S@Ë RõØ·â.Ì X;¸ãy$R25ÛfcvxÏt¨i€–>YƒVæ‹Ór›oÔ°k1öú"4›ß܆¸ýˆ[@ƒ£GÛÕÿ¥È¬°Šª–HºHdÚüuN:cÆ=pÒ±C»IdfN’/BõÏ×iiÿ¦v÷&Âî„—Ô;ÓNxu%&ÕjBËÓÐJPAb¿#±T=ìûLà Æhqÿj´Ü&ñ'ÞBÀ2züLãÏH•‡ÓZ§u@ýs­˜úS¦v×>À¡|ÿ9liŠ¸Ki˜Ž½Ö^ p›ìÑ0³ \;üÑf$~ML²_ƒ++Åi9¢þù©â.̱ØÜlLÃ…?›ê#‘’T¿zJÀ¢£jsë×kåû¤3Æ~ßJÔ¿W’/lÇTöÔk˜‹VRtµãï{DÕ›‚8×ÐѶœ°C»1©"én40¨/ã ³ˆÉiåHz ìÝ&ÎÜïÄ]Ò7Ϊ¸wªšiÇMíö±¯°CFýÓMZüx/‘ºß°Cõt/Eš–fƒZ›{fÍ"±nÔzØþfÇÔ¼ñ ,—ÄSoÿ%™PƒÎå}èÓqV¦ÆwÌ°©Ýþù­omaËl>-Ñe‡5¸:›KƒzØÒL[Ø‚¸9­½ÓЇsO/¶è¢å&‰?~æÖFøgg#±+j`Á žT¿vFà4Ú뫱¥´#ÑöoÔûÕWÿÂR´ãa´Ü46ÈLѾ·‰÷"Õ=bR½\lCW'6' Ð@&Öc}P˜1aÝÜÕ:z¬Q½Só±ùyhe.êÏF+Íh©­4‚7;–ñnìÐtÔOHbãn‰¯ëEâ¾ÍoZ‹iÍmÇõíÂIg"@ZŠkñÀ »æa‹-èÈðæ`Ë3¡Ò¬ê׀ɂ–@Fþ3À¿€ à®–Õjå‡l¡·µ÷ŽüSÜõ¥ÿ®~rý_ÀIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/graph.png000066400000000000000000000031271207742442300211540ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ÔIDATH‰µ–[l\Õ†ÿµÏž±gÆñŒãÛdœÚÇ“±±kìFil¥55R™z6BB•(!QoHþ÷?p¯=6ö§jj¶m˲nc „Xªêõõ«³³§~€GG¿ÿm)S!"r !–ÇBx…“`E£Ñ{´LNþòé\®å~fk`këh4>;KD>"ê ¢ýj‰¨ˆö_¼øÓg …ýñR©nhcã3-áð+Bˆ&>"ŠV´ÄíÀ6nwæwÿ 0…ÁÁ§»?Ζ¦˜}¾›?–òÖ«¯ŸjÿO ¥”OÊÜ)tèЫ/ÖÕÝx ÷ÒÒ_RJy15ƘšªÞÙé}Ànÿ/Î /h½rå['*5>!D1Æ¥”ò)¥|@À¸ÝÙùžž|cc!œ]f9 à.!D—¢«¢;§v¤¦&ýVSÓlhpð×›L¹\û…JM_¥>|»/ú°àRmíö3J©3òòïXVaŠ™‹Å:b±X3c`à©~€àïUï^”Ò~©Z£”º;‹¯Z$H<´¶öY &6t+¥š<žßnÚvpD)Õ/„€RÊ—Í>vƒo¥„p¹vW˜­~!DD)å#¢N"r+¥2„H¥z£Êmmÿ˜° `CÊÜ5cd`qñË%fÞfæm[;;½Ÿ€††ùIfN1s @ÒqjÛóù@À€4€ªÞÅbý €+~ÿÜuÛZë ˲߀¹¹“­ÕFZë-Ûné$*_¿víô"3ï0óN©T÷@rnît@k½ ÍÌ­õ–Öz“¤´íÆÆ÷Ž}ò2f¾eÛÁæ7ÞøÃW:;õùÞÞg߯ ý‡¯½öüW}¾õ‘‘'þ`lo¸‰'Oöô"êÐOD‘}û®§ îî ¢ˆÛýÁ(‘)F"­pEˆ( »¾~9eŒ e³‡2s$•êàih˜ßZë·µÖoÑe)å´Ö:ADIÇñ]¸¤Óy"JÚv°™¨cŒ·±1¹šÍv Pð÷cæî“ÒNc¼•½ä­Z =¸œN†ËukØqœK¹\S7„BS+²b„1"jB4G£t––&ìr¹öóëëŸÛ¨5XÜBP'„Uâë ‡ÏfgOf,+÷Eċ…ÂþË*® ý¬mÏ"ÓÞÕZ'\µ¬B’YœÝÝ=xÏìì£aðûßÿ€«ZëK®Vô4€+RÚgóù¦ÑÍÍ¡Ûn=\.»_ÖZ'öˆfŒ 3s±B±CÌ\ Icons of the Choreonoid base module image/svg+xml Icons of the Choreonoid base module Shin'ichiro Nakaoka v v a a E choreonoid-1.1.0+dfsg/src/Base/icons/perspective.png000066400000000000000000000025321207742442300224030ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<×IDATH‰…•KlUU†ÿí}¯J4-ä%” ÈC‘£FÔ0 :a‚ ãÈè'Žœ˜0 £ÁW  F@MäAž‹@Kx(p{ïÞëwpîm =“}öã¬oíÿ_Y‡’ðÉшÀêãïwÎWó¿$g‡RÓ[òôB˘©©©¹å i¤3aF«ÏiFX0Òxæ­£*7®”ŒŽÿÔ<KMoÇò©g?×1cÁ²ï{Ο?þ‘§öˆ¢Ä!Â!QÑÄplç×£ÎÙæ!–+9U- <fË,”ß¼gØp>ôġg,XþE,•­V½9ìòÙc ”DNÁT¨à‚QÄÅ®#Cölx¿}áò5‡6´²=§ªE’Ífq-¬4´íÁ™‡Ú&ÎêòT 60™FCF¹rãRSO÷Ÿí,42‚2F4ëؾ¶}æÂWOŒœ8ë* `ÀA¯7ý0Hʳ]èÜ7Ž$AAˆž«¥ë—»FRõu4’°Z¥·,9f,xåŒÀ@˜7°Pº¶èÏ×Jˆ ¥()È,RŠ©zcè_û|iòœÅ›UלRë>¡ûÄîÖm_½3G@2H  4›ÓÛsîÑÇžY¹²,²_k57 ª•åL`Nžj±÷òÙñ¶Ü8}ð§1ŸØµ|ÚÜ¥ïÆ!Ãor"˜\̤§X@ LNOr¤~HV®Ü¸ÒBi¿¤k¸fÇ£;¾\5~Æ“ïµ›ÙíRt&Xô£†¡¢ƒ…±pk´(ª(åê: z"HÞZ½uõ‡sGm=bÇ`P/bížUÀ0¢q»÷v,\¸É'bÄ·E¬Ë"Vô¬ ééë;“’¾³Ùœ=@.ðJÄZ:+À† 8**HݹsQtJJîãØØEŸ _Ž:kÏŒŠ ú hÊÏ'µ¨(&=-ÍóÄåZ[#b8€["V¹ˆåˆxñ‚_ÙÙ¼IH ¦ªŠíÛ¶mþ–˜¸ãbT”£8 ¼±–E (€ßOÐãáõýû|(-eaaº;9ù¨5gŽë°¨±öETÁh¨¬Œ†Ü\®mÜȺšš¹9[·¿—|Äܱ®ˆXŽH€‘ÔÖÈÊ¢¬½Ö7(./?ðÕåZ³OÄhŽgüþMÈãÁ{ý:%v;¶¨¨Ø>¾˱¶)>G'ÛPRÂGŸïΊîîO^P'p8# ½ýyZ[ÛÓ-ª¡à.pHÕì½'"@5hø|ÞÌžž†åŒiÉØLP€Ÿ1ß¿ßÎô»·d·ªY7Ñ¡iUÐÙù~^KKõò¡¡?v&hID€jHš›«\@òTÍKS9;% xkW²_ß’ [2. öJeeÕÕà÷wþ÷òá †¼@Îd-³ªz®««côoË™ƒwÀàáD·d²ü1Ññ/wIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/projectsave.png000066400000000000000000000020731207742442300223770ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<¸IDATH‰­•oLeÇ¿w×óÚr¥]»&ˆÌtihÍ2YŒ¨ö…Yÿ$Û; cöB’ùfƉ«Kô•/¶—83¡“B\ât‰º%›†¡ÀÀPX%+ ÐÒ×ëݵw/X ã® Q¿Éóâ~¿ç¾Ÿç÷{ž{Ž"„`·.RÔKhCbK B«ä ²˜í<í¹®.`cuÕò×ÜÜ%þ'@Ï˯œ8qpw<13ƒÅD¢Ze{@×I2Æ0ié¿äóÇ(êb^,cÉè¿ø@u5™T7·ÖSLåóï~·'E]²}ºÏê®Ö¢àk/„{Þ~9<5%`hèiÀÄÄ¿0|Ì0}6Žk,zÁÓÆÊUH÷ÿ@vø*ä„ÿÒ~h ±¬qû(êbøѬqB"BeÁŸ0ÌIŽec¯¶·Ûhzë€XYÍn·Á_R,e³•çù¿×ð‹àÁ7®÷ÑpðYd2KñøÍ!‘»å9ôùR醦iç'<Øðò<¼orùl_M¦0ýâ èlý¶ ¶©©íÛÇG“.×À°c“EU=ó[*åuÑløˆÛI™m”Á—ók˜>CÑÞbÈû|O3nßš­TBt±X<9œÊ¦3¦-‚$apa qß›ØtJH§GQ(Ðõê÷c™!…˜»"º®#+óï¾³gðùÝ02FOÏS(•ÔªæˆÌ~uºã:®nÈwôýž$AéìDÇ… MË!›!ÆΚ֦Ôè<‡Š‚.I˜—$üy躣QÓUúýN0ÌúÞ*(Kzòy´õ÷#FÓø©¡§ÆÆ@[Ìo–²!Èfç ëZ%Vóö¾>ȹ½½àêëkM0 ¯W€ª«t]cdYM«XYñÌ[gQPX †¹œ E‘À²ð<‡Lf{Ž ŠëŸÝº½YW§¿~ü¸p”‹ÅØMB€P¨Ÿ¯ ƒƒ+¦U¹Ü;Q°Ù.Ïùý]QMKb`à(\.«©AY{”¥ëJ||üú×,«5†B3]g‚¦ex<æ «•CK Ýn­Ý¢²å½I=ÐÚzí÷¶¶7Ž¹\?âÊ•pÍJ"ÛimÀNårk‹wîÄD—K8ÒݽV󧿼¼I­®&ã²¼¹ÿJ㶎 1‘oIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/refresh.png000066400000000000000000000027751207742442300215210ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<zIDATH‰µ•[l\W†¿}Î\ÏÌxöÆõøQêa¿•4sWOññ[?–µÂEÐe"±í›l™Á¾íõ·º‘ØÀ¹³r…ââ$UZsþ@ÛííÑxË¡FÎAD¬rqŠ¥ì¿jn­Œ³ò)èA0Ò¥Qêp³@«/7ÑNC L©p™ZÂñ^’tì&`…mä&gA:Pꊳ:³•Û4ˤUǦMMÐÃB¡Rš'¿l®îØ»?I¼¾ÁPkoCà˜’\“½ß¯”‡Ð%>_@1 çeˆvYÈM³°¸ÈB>O¡°L]÷†÷ß}Ÿ‚v@°w}¥çåvd*N¢8ñ|tªâ8ÿ^-ÖÞ}dœ˜?èU[樗6µËêÅáøC*ð¥û£${“$ºî%Ü2 ƒ@?àf)àÖŠ—Èç.³0ûŸµ+‹Öü´2Ø7¾ÝÛ).}ÊüÌE€„’¼±w˜_%[©ë¬-WZ ¨5ŽÛS;,À"Ô§²nV¯}Pq,ã[‡GØ¿§‡Zee³ƒ€'bñ‹ˆ%?Htö±¸äçôKïÐ1e÷Á¡–N`Šòêç&þqó£ çg[ýÅÌHíHKK«îîl3Ðe´ërsiÆÎÚ)q ñšfÒêM ´oûÝ÷™ dsEÞ¾°ÈÈøÜ3öu Æ{o¾êœ;sâJWtñµÁný °/i•ž¾Êh¼…|Žùù€/Û)™ØtÀË•õÒþ¥×hkë ;é᱃I^??Á=cã@•'ÿ¢-ºÛëXˆ€®mý3ë•×gnº®«×€CvJÞnøÌFˤUxز|ZkŸ1§†0L‹ï‡t&LAJåËÅ*+¥š!|ÕNÉen‰Û­Ìð=Ï t]škÅ4½Ü?T`c˜ÂjI»uX^~f§${+ùmš„<ÀS¦ü?G™Á±rð!xÎ'€?Ú)Y¿-Aó nzòµçL#”þá'姟Gt§ø/Tu;£%%JIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/resume.png000066400000000000000000000014461207742442300213550ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<£IDATH‰µÖIHÔaÇñï3:¦ ^Òlu&3+lU(Ñ›@ Ö!"B0"‚"*‘ ¤0ŠèСíbÛ¥.EE´F¨ÙæÒn9šŒV¦A¹ä¬ÿ§CQiÓh¿Û»<Ïçð>‡WT•Ÿc]Q’«ªE"F‘ãÖÁÇŒ0¦?ìåKSµÅfßô?€Q NZ—ÛËãíˆu`\l<áá¨P`Ž ¯‰·Ù-£ ˜Ía$ZSˆŽŽtNÔY——¬5`rlëT¦''a2I”Š^±ØJöIié°u&¶¯]@a^* q“ÈZ¸@"ÇFº'±Ú}#yé®è†*%Çîáöø±,bZÂVf/”øÉãAÕæ3‡ÕY²w§ øüço¼àÒÝ&6åÏ%mÖr—¥“™6Ð81™ªWØ7 æYsG/7åWùÚç"ÙLæÍH hcŽXãb6˜#ÇÔ&e[ƒT•šF'UõïÈšŸÈìä)æ›9 Õ }bÉÞ40˜÷]½Ü¬yƒÈïg!†×ÿó:4ÀíñÑðºs—«p8»žšü¾ü·•‡ÚGhqvrñúíp pÖÛïÞÜRsdà×{A÷뚨züŸ ÛZo81ÔÝ</×këiëø„ˆ8Õð¯i½sðáp5_¾öò¨þ¹öDn‡z=ëš*wÿ­.  ãÃGÞ¼mVÃP@ö·fŒÙ«{ËŒ@j‡Ô0hq4ÓÝÝÐ#*ëe×iüWÀëõÐêx…Ë5Hƒ]ÝVQÖò/͇>w¶Š(§=ß\[œÁˆzP¶:*œ ¦ñ€aèec§ãöÈ¿-ß+š¡›`ÿIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/scenecapture.png000066400000000000000000000020621207742442300225310ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<¯IDATH‰µ–_H[WÀçæ^“˜ÄükˆmýC‹DmÔ•–Ý ´}Ø:jq¶e¬}Ü1(c/]ñm/{Âðq”=Ltl+e/²?T|pƒ>tˆi«[›Dsu‰&ËnîÍ={P[çmËúÁÇùÎù¾óýΟ{?ŽRò"E}š !Ú]Õ= †Üc…{‚Á£Ÿ9ÒÊãñ4Íù—mW´JŪ³,SK&c÷€‹Ï p¹úN'ûÏŽžÓ{zz2½½½u]÷¥R©@*• NN~ß³oß±Q]Ÿß Äy?à~âZ …BîÕÕRþÑ£œËåÊÔçó9w6›se³E­»û ³³÷ß×´W±¬èý's5CÊ/þ0 Â7<_§ÓY/¤)Á¶m‡¨R [J!7)¥Ä¶Á¶…”Òތ؇JåoÊåü惡2¬úýÁ††Þ:ÖßÜaš`š`YmµþÓÚºþkdqqbLQñF"qÔ±Û]<øý}ä „BÙ5X×ïL~ÅÒÒÏ€ Ün7.—‹x¼•lö&Û Æv»& •ú‘ö£iN§¯×K8Æét¢ª*Š¢ „  “ÏOÕÜAÍKV”:,˲, Ã`}}Ã0(‹†išýBÔï ØYc±W¸wï'ºººÈçó8ŠÅ"kkk‹EJ¥éô2À嚀šGtèÐY¦§ï ë:Ùl–t:M*•bii‰••r¹¹\Ž¹¹4‘Èk{ï`§44´ÒÛûׯÎ… gñù|!0 ƒB¡Àòò2·ný@{ûUœÎ&Ló}}ï`Y’ññO9|¸ƒææ&LÓdqñîÞ}H[Û‡Äb—k&ß”-«ŒªjU‰wik"™üŽÛ·§Âá·|‡£˪žXÊ -‡ÝšmËqUJñ¸änWUõ¿Dkë0--Ãøý(Š·jì†J2™›V¡ðÛgFÜ>_à#U­;¯(ŠºAßZÅ¿í­mÿwlC+¶m[7L³8&^ô«b÷:ý?È?÷ŒÊ”Fè»,IEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/sceneedit.png000066400000000000000000000023611207742442300220150ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<nIDATH‰­”L”uÇߟïóÜ=wÇyGp äƒùœTZÔœ›Æ‚ Ö7Ýj5\¹–+Ö–6mëŸæºÜ¹æ¬a‹¥9²-ÅMq: Ne‰]qü¸ã~<|Ì„JÓÓ­Í‚ äiš>({L&¶Ðê‰Åø¯=©ino?4|_@]Ýîõ¹¹Ëضms!®‹000ëìyí™3n…8ç›.]ºZ84tãÃòòg­D"€ˆ Ë Àd’ (Q0Æà÷‡™iA×uººÎÏ^¼øËÑ#G>i¤ï++9ei7&IØtì;ŒOç’%¦.¡“1À±oŸÛï÷O­Å[·°Y\ÚV8¥ëדš%šÏ×âZ‹ŠÖˆ==½e¢ær¡ÕëE¹,Ã)I‡qÅj…Éb™k¬ƒØ¤ªPãs1×(”\[µÊeíïÿ½B¬ëîÞá9{öÈô®]‚S’ðÔþýšÏ‘{ÁáÈ(ÏË{Œf½cèmn†Õ`€`Sš/†(??‚Àž|ï u¸\ÎÞP(¼¾ç­FcÄçCEË—À™Ðì¢îÖ²³PUmEŠ|€Þ½{˲ep]G^ b J(Œ˜dýO=© †@AZ ŒDçT÷Ó“”ŒR#‘®X,L&pÎQ‚0÷–¢g~\‘š@á¥è…[¶ìðÌ™íÛ+#>Œii€ÁƺžH´˜žxM×uUð8÷®cà2€›U­­£W¯Õ=#9ž¶6ØV¯Æš×ß„¢DïóæSLLL16Cššø_—/ãéÛ·‘o6ã·P}ìYYIDÙãÁ‹ºívTÿ܃h4¶è%_¿~“»ÝG'#‘È«¢·­ /H3›ë¬VÊ20:š &ˆï¨d÷Gÿ ïè87éóMýé÷+u'O~6*úd=f3pÏVM9w¿Ë±::~œíì쑉Àã:I8'bçðÇííîk‰ßþj}ºaR&ÁKIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/setup.png000066400000000000000000000022501207742442300212070ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<%IDATH‰¥ÕHgðçyß÷l´±fF¥1…ú³¥M[ü••Òµ¢ml”iì`E»•MJa]÷G)Ü`û£ ¶•AËíÖA™3Rt2ݺ–Ö1‰ÖU;Ô”àœšÄ†h¦†ÄÜÝûìuljcýÂýuÏ=Ÿ{žãîˆ`¥ ºÒ @þ@ÔúÛŠ…«W]õœÃ'’‡BÈ _Óà}"÷õuˆGìBè_¥¤€m×®¸°Û7kSSþèø¸^/çñ8Ìè:‘¨ù^"€XrÇÉŒÁ{BÀ »]çee™zYÙnE¡ttt„­Vu!Dc0ÈMKt‚G€à6›åsåå©|ïÞÝ<55U{<ž?½^nMJÀ/¤Œ½CÔ>¿f€1°äæj¸ÿîóùb]]]³³³ˆ6Åbø»®Ã!¢o†mü?€ˆ$ªˆ(ü~ÄïG~玲Q×é‘ûÊ/D¬Ú`|Š¨%°°d—¨‘*¥Ô9çêü<"4µ.Ó¼¦’1ã}DyÜK’RBœˆtEQTT˜XZŒør¶®6EÁ«;wª÷‰èÊ] XÚ@'B•ˆtƘ&©Dò¢ë$ogL¯a š²³µûfý¤ª±9¯7⛟gE0–À(‰ .¥ÔŒF#nݪM8Ú´Ù,O!꣑N:âöÙ³ÉNg¥3##ÖFAÆ t €:66æ+,,Ìs8V¿ßïÿcbbâ—¢¢â<›ÍVFÜn÷çÃËé€x€:W½ÉŠRsfgË[¶¨S6›y±   ËjµZM&Sf8žêé鹇gÌf³áòå@y4Šß¹ßL@<â`L¯ÀW’’H·XädNŽî1™Øœ¦iÑP7îÛ·}SIIIÕÅ‹­WnÞTë‰ø³DÍà ÿBˆ5{8‡z)é%ƒ0†2§·mÓ¿<}úy;c šš:<““¢˜h¡˜¨sq9`Ùo QËÏšÖrœ(=+Å·#ü*FFø ×®Ýø.---¿¡¡d69Y†KýpÅÑšÆ\çRRªûßmkk›©­}ícÕ>WÕ²õ+?žå#¥©1EqéÒpv(úñðኣ99Ú9DºÊ˜ë¢«ö±+Z-DŸ©DP°š :o)Š’~æŒsóŽ‹uYYÚ·ˆp±ÖõÄÀßHË(‘l¼{—5ô÷÷7Y,–×È›+-Õ®[­òUú±ºð‰‡¹-%¤ yOOO9ç±¢"õWΡ€å¬`"0ƒLéííZXX˜Çã±în¾]ס@úVùé'Äšƒòü|­.3“Íöõ‰7âq:@ Dîöu‘ð-`Ô Ž}íûçü_Ì?#pæPýIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/stop.png000066400000000000000000000007261207742442300210420ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<SIDATH‰í•»JA†¿Ùl²¹n¢ˆ‰ASª¤ÄÚF°¶È#ˆO!¶VÄBÒYè+h¡ ¥…ÁØh—Hš+&Ç"Wt·³ÜÌ ßÌwfšQ"B!£Vv l"ÐÑ„lúTr·µàQl ìx­K ÅaúD zmXµƒ Š  Ã¾kÊ æ€ @þp”hjü‘^êoE¬» cìF&€/ƈÅÁk€×z€Vù‰ö{`²ßÌ(FÈdj~ ÌDgÀLðy=ü‰9=K|y½ÇÆ’MòzyÌÇÙÑ蹜žå¿â \+p®ÀÀ¯­aU(å¯Àꕤùùâ¸ù«\¢ss¾àoˆ"«„”‚ˆüâ`0W]¶ÅCÁçÐLMÓØøvcXšM€sIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/velocitygraph.png000066400000000000000000000034161207742442300227340ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<‹IDATH‰µ–[lT׆ÿ½Ï93gnöÌØ|e_ðdR Ƥ¨j£:s¢J­µ…Ñ©} f?­‡½õëÿ´ö¿Bàó<òJ¡iZÀÛœp{÷¤"ÿzÀ)„ȆB¡IMÓÀ[_㉖JÉÎå·þ|n  B,Œ±±`0ø !ÄÁë »+¹;ÿKÉW…9ße;½`”$i’sJéØJ½éð‚¬xÌ·`¦'”G'N¹dBˆ…R:nƃ”R;¥´sÅA­TwåïMýHð€gWúd¬Ë±_d‘K)-ºoÒF)-TKu·Åg¼  –êïQJDQ8%I*'„XÄPÊ»Z| æ'Šð ƒn ˜R{{Û­‘P`$ÒÐÇ»àVÁcñm ° tC`Ê¢Ò(Ø8ÉÛP@sJVù-¸Þ™@gHü´¬¤ùOœóÇî]$‰!ËAµØx¤ïÌðã;ª*ëUܼ’áöå·Üð¬Ã‘d[©Þ8Ò=·ew£Å9»ö¹_ ¬vÁš]_~ZñŸœZ½ŽPŽøôÛî9kÃäLb8†ý/U‚R ñË9¤¡q©öO þ°ÌÑ4 €Òâ1Åp¢«ÛXxå7Ñð¡¯Þ1O¿]P,5ÛUWßøÙ„û‹I Š+¦iöÙ\®·g^^»¿ãa'‰7MM…zcaª/HÚÖoû±ã{õçjÊZž)p×µ´žšé_}´u§N%÷[„¢ôiŒRZ»ì²5,K½}rÝ—·ºS0te´¨(X¡iZ•]¼ŒpÏ›¸i÷ÙcC½Ë«[¶Û‘1µr®¹ `¢K1'„˜³çYû«¿^Š;€÷ÏÇaU\DÌR"‰bé¶å2€{Œ±çÛÐPßš@É& 877Kª¨%ϼ „˜BÌ 7æãxøÑ\ ¤ñßw—=.c,Ê›•!`3’4–¼e?B¸¦iI[¡Ï95YPíù…J=‰gÈòèï}?–$¸`èÅu~òG¿9 oNUo®³pÿá×÷þŒ(B‘Ç^ÈÚZ`ä ! À"ÕýSgë×gJÑF‹´ËBâ¶OŒ !<=îÊN|¼ùÐó~œøõ4æ¦ÔÞò’Í!d[ ûjnùŸož}ZZZü²,ë­­­SÜ[‰è¯(ö[°8½ ¯_½Ä{¡i_C}Zô¿ûÜ ¿zúÅ(Â]iž–õ}„¿œ—õR‹8`YÖ4Í„;]Ó4ç.žd¤\¶;)À»AÁLL‡î¡£ Í>Ó¸{òç'JÊÚÞŒ¡ý\ÂÜó“Z I{ÑäIØ\ÕÙJ€À  @!¤@€­„ 5Ç’šŽdW¸ó$d:¢W ‰b¢ó™ãE mq°7b»~PÛ¯º ú5!D…ääÛÀHÐYz?3®B®Ë²Üà Bz %Ñ¥sMÀ+#6’S7nþÅk%Um‹h}}þ®$ñF§Ïú>8«2VEQzí~CÙÅ;WqΆaÜGÄí d.¹d–¯9áirôµEýà ºÚ·ðÔ>éóbúblÄjã&¶†qª¼ Ì•ï#¥´„sž!„ø(¥>‘•ñµß¨Vqìx)ž;ÁÀµì²’=¿S•7€êøMkÊ÷ˆ€4«Ù&Ù?E}ϵ¸Š@€Œ±0€¾äìò•D| ÑL$‹§¾;Žþë™ëKúCª’sŸÆxÏÂ5[HpDˆ$òKŸˆ5 *€Þ¿ýö|è3ˆ„Ê "‰Xbwï¤!pù_‹xé—Ÿ€ÊÎÅ»Ë~™rÎí išsn7¥sŠÇ|Rñš@è伦iŽÏ BdäQJ}…ÞÚÉ®ö°9Ø;,ÍÇcà/òHM€2êºIg¥”n˜~'÷Rñwb;©*¶e£rÏä)7°•¬lš¦Öz!D¶ìì—Ë3³ãû[&^=ôÁ™ÿ·¬ ýÍÇf§ÒåÙÉÓîöÖwB—€|ÞkËÿðér„Ŭ-IEND®B`‚choreonoid-1.1.0+dfsg/src/Base/icons/viewfitting.png000066400000000000000000000026241207742442300224130ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<IDATH‰•”L•eÇ¿Ïó¾—ˆzÜ A èjȈ^S’)þnþdÙŒ4³Í´¹Ú˜šÉ&¦¸~˜D–2ÿpæš•Ž¥f ’2%RÈ‚-ðl‚¡y/¼÷<Ïû<ý—‘\ÔÎv¶³÷9çû9çì}h­ñ}çÎÖº;š¶>J.ÓZÖ=ù„ièIZ)ƒ1n*(SkeppSsm00Z ÜŒ2lÒvôtÛ3KK³ëñ¼ºeòE³†“Zr0Œ)ÎaLp(9!2BìóÙ¶}UJá-+[ ƘÂÀi)Ë æÌ{:sŠ¡—\pé!ç€ëð3KnþÕÛðYå¶m²mQ d,¿ñ®f8WÓø¦<< Ì­ÀÜÜ p77^OïìèIQŠ½VV¶ ‚Ȫ&²¶K{(`ÿþ?PzóíÞ{/µþq#}HØà= ðh…ت³õ38ç_x§íYëˆ,AdU,_~?Ÿì¹VÉ«¿©_àQ€§ö|Ó3wïøÇ1†7‚ùGŽ¬¸Fdm#²fY fÛ›únûÒ~nÍRƒ«r[VÜùšÆdhì./ië™Od•YµDÖ®¼¼])”—^oÑÀG5ÕWæØÂŽe€çÜwWR¥´ï¨Ç¬½÷çWVnTDÖZ"Ë$²Žj8ø›më”)Ï‹q*®?Ÿ8svšHÏHÑŸžv™mº<â}q°-Þµ§½ý§‘u³goÝfÛ¢DJ±öòå}‡Çl™<¹@¥bB Ó˜úx|4nµöúÝgy³Á˜)”òR¿—´·ç¬ËÊÚlÚ¶¼,¥˜dÛ2­¹¹¢;äŠ 92rçîˆÌeNðö®Î[X4¸ë­iÓoÉÈ8>Ãín䌪»téId­!²"‰¬ƒßGã ±aœ'¾Ò™ü~AoÒÞyÿx$€$Ia¦…hj:Ô@d½Gd-NHXTŒ¸É#ˆl|XXžÞ?!Ðz€‰`ÌÆX´øB€È*¶m±DJùQTÔ¼êÑhÍ#ŽØ`ÇCžŒœG³1&\@}¢Ãqó["Ë#¥ïóÑ0ÆœÇ;†Ö.Ãn2æ %?=66¶<;;;9???ª¢âŒ®«kË` 3ŒXj¤ð‡ì<33󫜜œç cœN'æ΋ððptwï6W¯n2¤$uôh¦‘ñ{DIݦŒc`Åî‰ÿ¾Ì‰‰‰^¯77---jýúõ€ÜÜ\¬Zµ .W„ËÔZu(¥Œ‚‚‹ß ¨zÊ㯼q#y„…˜²/»\®+W®Œ***>¯©©ÁÂ… ¡”‚©”Ú®”zA){±ÖêŦ”"Ð&UIIUBйºº5‡Z 8Îééééðx<èè脇‡Ãï÷Ãçóußä¥Kp AÏ ˜/åII^!È" ¤¤_… *!UBÐ…«Wß¼^oí‰'²[ZZP\\ ˲°aÃô÷÷JKKWz*‚–“sl¢\))OÊ"*AJ A†`U))µå§N}žžÔ××ë“'Oö444z{{ ¸ßRS÷&HIó‡`ó… x) ¶=`{­C¹œÁ¡«npÙˆr]?ýp\ENÞ\6G‘ ÃväÂHéÇì­5ŒT0:¢Ï&³/Ç`*,idÀjþ:©s8ùÀ"¯ßçk)bÊ!…p„Bpɤ$'¥Ä¹Pö]ÖÀ@ο„gÊÆÑGÚZ ]O†££ñpÏ៛Ë=žmUÕþ7½טD„Df³2VÆív«¢ƒHˆ˜Cäs|Â[é]ѺæM׌Hl$îéî½WeŒ‰§^ztX2y„Ò·t^c}Q‘âyöåÕ%M«’2ÀÀ@;Ý?ˆßtvù6´¯ý^Ó@: )Aûì“Nz·ýÃwE¡§×®–LžP„Òwó¼†[¯~É9®­›v„ =U%BB ï•ÂˆÈ½õõ¡dÒ(»M„¤”µH-Â[Öý.™ì›Š(tůUU¾ñíIÖTD7ÓC_w¹OˆeÈ)ˆ3ɤ!¢cKž|pǶ³¶mÛvƲmÓ´”æ5-dp×ý+xÓª @jÿ´ä¼„än_¿­ÆHNLÓâ‰ÚÓ"¿±~[‘LMÓbz”Ò£ª.•·oõÜ%lºR—KÍmžbMuPó¿üEì·þ3å–™öÇ¢‰Yd1I©½·e§½óý=ªãˆ€p9Î\Džãê¡ýÝ× ‡ˆÈq!“ ÏåtÿÙrË2ýÑQm6p³YUwluH,A$äÈ 9ÅŠK\ù*+^A>¹Tpò’ñh…Ï;îr¹ÚˆÈA$""‡Îñ``†¹@±‘#Ïá(Ë·Ñ‚•¯µ=÷𺥷,:•IÛC†n ŠìîØ÷øŒ™ÁÈšç[›çÌ¿vÐ2Óçu-yþÇîcVOWo=}å‹(¯ªªƒ´pQ­ «[xCxwÇ>˜º.W¿l±QÛ™¬ÑÓÕ[_¨ƒ¼:òÝQωcý Ì kzR7ÄÇ5@–J Ê—ûø¾Ý¤ÎL™¤žºq$2–…Ë« Aã·YÍ÷-?ƒ00üÇèÐÝ··¶3¢¥õždý²Å0ƒŸîúÜØþVÇCù8W}Ló–ì÷6¼0£:¸±ÜS³íl:meÒ–™¶SÆ„Tùà¿Åív'2ÛJ[™tʘ–™çįÏüõ'fR®‘,ÌOIEND®B`‚choreonoid-1.1.0+dfsg/src/Base/po/000077500000000000000000000000001207742442300166455ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Base/po/ja.po000066400000000000000000000503771207742442300176130ustar00rootroot00000000000000# Japanese translations for PACKAGE package. # Copyright (C) 2011 THE PACKAGE'S COPYRIGHT HOLDER # This file is distributed under the same license as the PACKAGE package. # nakaoka , 2011. # msgid "" msgstr "" "Project-Id-Version: PACKAGE VERSION\n" "Report-Msgid-Bugs-To: \n" "POT-Creation-Date: 2011-12-28 17:54+0000\n" "PO-Revision-Date: 2011-11-12 11:42+0000\n" "Last-Translator: nakaoka \n" "Language-Team: Japanese\n" "Language: ja\n" "MIME-Version: 1.0\n" "Content-Type: text/plain; charset=UTF-8\n" "Content-Transfer-Encoding: 8bit\n" "Plural-Forms: nplurals=1; plural=0;\n" #: App.cpp:158 App.cpp:317 msgid "About Choreonoid Framework" msgstr "Choreonoid Framework ã«ã¤ã„ã¦" #: App.cpp:320 msgid "Choreonoid Framework Version %1\n" msgstr "" #: App.cpp:321 msgid "" "\n" "This program has been developed by Shin'ichiro Nakaoka and Choreonoid " "Development Team, AIST, and is distributed as a part of the Choreonoid " "package.\n" "\n" msgstr "" #: App.cpp:326 msgid "" "The source and some binary packages of this program also include the " "following thirdparty libraries:\n" "\n" "* Eigen (http://eigen.tuxfamily.org/)\n" "* IJG JPEG Library (http://www.ijg.org/)\n" "* libpng (http://www.libpng.org/pub/png/libpng.html)\n" "* LibYAML (http://pyyaml.org/wiki/LibYAML)\n" "* zlib (http://zlib.net/)\n" "\n" "These libraries are used and redistributed under the terms of their " "licenses. Please see the corresponding directories in the source package or " "their official web sites to see the details of their licenses.\n" "\n" "This program also depends on a lot of other external libraries, which are " "linked into this program during the execution. Some binary packages of this " "program may include the binaries of the following libraries:\n" "\n" "* Qt (http://qt.nokia.com/)\n" "* OpenSceneGraph (http://www.openscenegraph.org)\n" "* Boost C++ Libraries (http://www.boost.org/) \n" "\n" "Please see their official web sites to see the details of their licenses.\n" msgstr "" #: Archive.cpp:87 msgid "Warning: ${%1} of \"%2\" cannot be expanded !" msgstr "" #: DescriptionDialog.cpp:28 SceneView.cpp:294 msgid "&Ok" msgstr "了解(&O)" #: FileBar.cpp:34 msgid "FileBar" msgstr "ファイルãƒãƒ¼" #: FileBar.cpp:36 msgid "Save the project" msgstr "プロジェクトをä¿å­˜" #: FolderItem.cpp:60 msgid "FolderItem" msgstr "フォルダアイテム" #: GraphBar.cpp:88 msgid "GraphBar" msgstr "グラフãƒãƒ¼" #: GraphBar.cpp:97 msgid "Plot trajectories of the target data on the graph view" msgstr "対象データã®è»Œé“をグラフビューã«ãƒ—ロット" #: GraphBar.cpp:103 msgid "Plot velocity trajectories" msgstr "速度軌é“ã®ãƒ—ロット" #: GraphBar.cpp:108 msgid "Plot acceleration trajectories" msgstr "加速度軌é“ã®ãƒ—ロット" #: GraphBar.cpp:113 msgid "Show options of the graph view" msgstr "グラフビューã®ã‚ªãƒ—ションを表示" #: GraphBar.cpp:130 msgid "Limit" msgstr "制é™å€¤" #: GraphBar.cpp:131 msgid "" "Show the lower / upper limit values on the graph. Trajectory edit is also " "limited within the values." msgstr "データã®ä¸Šé™å€¤ãƒ»ä¸‹é™å€¤ã‚’表示" #: GraphBar.cpp:138 msgid "LW" msgstr "ç·šå¹…" #: GraphBar.cpp:147 msgid "Grid" msgstr "グリッド" #: GraphBar.cpp:147 msgid "Show a grid on the graph background" msgstr "グリッド線ã®è¡¨ç¤º" #: GraphBar.cpp:164 msgid "Ruler" msgstr "定è¦" #: GraphBar.cpp:164 msgid "Show rulers" msgstr "定è¦ã®è¡¨ç¤º" #: GraphBar.cpp:173 MainWindow.cpp:295 msgid "Edit" msgstr "編集" #: GraphBar.cpp:173 msgid "Edit mode on/off" msgstr "編集モードã®ã‚ªãƒ³ï¼ã‚ªãƒ•åˆ‡ã‚Šæ›¿ãˆ" #: GraphBar.cpp:179 msgid "Free" msgstr "フリー" #: GraphBar.cpp:179 msgid "Free line edit mode" msgstr "フリーライン編集モード" #: GraphBar.cpp:186 msgid "Line" msgstr "ライン" #: GraphBar.cpp:186 msgid "Line edit mode" msgstr "ライン編集モード" #: GraphBar.cpp:192 msgid "step" msgstr "ステップ" #: GraphBar.cpp:203 msgid "offset" msgstr "オフセット" #: GraphBar.cpp:214 msgid "CP" msgstr "制御点" #: GraphBar.cpp:214 msgid "Highlighting control points" msgstr "制御点ã®å¼·èª¿è¡¨ç¤º" #: GraphBar.cpp:222 msgid "Sync" msgstr "åŒæœŸ" #: GraphBar.cpp:222 msgid "Cursor movement on a graph updates the time bar position" msgstr "カーソルä½ç½®ã‚’タイムãƒãƒ¼æ™‚刻ã«åŒæœŸ" #: GraphBar.cpp:232 msgid "off" msgstr "オフ" #: GraphBar.cpp:233 msgid "cont." msgstr "連続" #: GraphBar.cpp:234 msgid "page" msgstr "ページ" #: GraphWidget.cpp:1136 #, c-format msgid "Graph: Position = (%1$.5f, %2$.5f)" msgstr "グラフä½ç½® = (%1$.5f, %2$.5f)" #: Item.cpp:630 MultiSeqItemCreationPanel.cpp:27 msgid "Name" msgstr "åå‰" #: Item.cpp:634 msgid "Class" msgstr "クラス" #: Item.cpp:639 MainWindow.cpp:292 msgid "File" msgstr "ファイル" #: Item.cpp:642 msgid "Refs" msgstr "å‚照数" #: Item.cpp:643 msgid "Sub item ?" msgstr "サブアイテム？" #: ItemManager.cpp:37 msgid "Name:" msgstr "åå‰" #: ItemManager.cpp:91 msgid "New ..." msgstr "æ–°è¦" #: ItemManager.cpp:94 msgid "Open Item" msgstr "アイテムã®èª­ã¿è¾¼ã¿" #: ItemManager.cpp:96 msgid "Open ..." msgstr "読ã¿è¾¼ã¿" #: ItemManager.cpp:98 msgid "Reload Selected Items" msgstr "é¸æŠžã‚¢ã‚¤ãƒ†ãƒ ã®å†èª­ã¿è¾¼ã¿" #: ItemManager.cpp:103 msgid "Save Selected Items" msgstr "é¸æŠžã‚¢ã‚¤ãƒ†ãƒ ã®ä¿å­˜" #: ItemManager.cpp:105 msgid "Save Selected Items As" msgstr "åå‰ã‚’付ã‘ã¦é¸æŠžã‚¢ã‚¤ãƒ†ãƒ ã‚’ä¿å­˜" #: ItemManager.cpp:107 msgid "Save All Items" msgstr "å…¨ã¦ã®ã‚¢ã‚¤ãƒ†ãƒ ã‚’ä¿å­˜" #: ItemManager.cpp:112 msgid "Import ..." msgstr "インãƒãƒ¼ãƒˆ" #: ItemManager.cpp:114 msgid "Export Selected Items" msgstr "é¸æŠžã‚¢ã‚¤ãƒ†ãƒ ã®ã‚¨ã‚¯ã‚¹ãƒãƒ¼ãƒˆ" #: ItemManager.cpp:348 msgid "Item$" msgstr "アイテム$" #: ItemManager.cpp:351 msgid "Create New %1" msgstr "æ–°ã—ã„%1ã®ç”Ÿæˆ" #: ItemManager.cpp:392 msgid "&Create" msgstr "生æˆ(&C)" #: ItemManager.cpp:395 ProjectPathSetEditor.cpp:120 msgid "&Cancel" msgstr "キャンセル(&C)" #: ItemManager.cpp:618 msgid "\"%1%\" cannot be loaded because the file format is unknown." msgstr "\"%1%\" ã¯ãƒ•ã‚©ãƒ¼ãƒžãƒƒãƒˆãŒä¸æ˜ŽãªãŸã‚読ã¿è¾¼ã‚ã¾ã›ã‚“。" #: ItemManager.cpp:621 msgid "\"%1%\" cannot be loaded because file format \"%2%\" is unknown." msgstr "\"%1%\" ã¯ãƒ•ã‚©ãƒ¼ãƒžãƒƒãƒˆ\"%2%\"ãŒä¸æ˜ŽãªãŸã‚読ã¿è¾¼ã‚ã¾ã›ã‚“。" #: ItemManager.cpp:643 ItemManager.cpp:644 msgid "Loading %1% \"%2%\"" msgstr "%1% \"%2%\" を読ã¿è¾¼ã¿ä¸­" #: ItemManager.cpp:661 ItemManager.cpp:823 msgid " -> ok!\n" msgstr "-> 完了!\n" #: ItemManager.cpp:661 ItemManager.cpp:823 msgid " -> failed.\n" msgstr "-> 失敗.\n" #: ItemManager.cpp:679 msgid "Load %1%" msgstr "%1% ã®èª­ã¿è¾¼ã¿" #: ItemManager.cpp:682 MainWindow.cpp:702 MovieGenerator.cpp:97 #: ProjectManager.cpp:394 msgid "Open" msgstr "読ã¿è¾¼ã¿" #: ItemManager.cpp:683 ItemManager.cpp:858 MainWindow.cpp:703 #: MainWindow.cpp:947 ProjectManager.cpp:395 ProjectManager.cpp:419 #: SceneView.cpp:1849 msgid "Cancel" msgstr "キャンセル" #: ItemManager.cpp:700 MainWindow.cpp:707 MainWindow.cpp:951 #: ProjectManager.cpp:399 ProjectManager.cpp:423 SceneView.cpp:1852 msgid "Any files (*)" msgstr "å…¨ã¦ã®ãƒ•ã‚¡ã‚¤ãƒ« (*)" #: ItemManager.cpp:804 msgid "Saving %1% to \"%2%\"" msgstr "%1% ã‚’ \"%2%\" ã«ä¿å­˜ä¸­" #: ItemManager.cpp:806 msgid "Exporting %1% into \"%2%\"" msgstr "%1% ã‚’ \"%2%\" ã«ã‚¨ã‚¯ã‚¹ãƒãƒ¼ãƒˆä¸­" #: ItemManager.cpp:833 msgid "%1% cannot be saved.\n" msgstr "%1% ã¯ä¿å­˜å‡ºæ¥ã¾ã›ã‚“。\n" #: ItemManager.cpp:835 msgid "%1% cannot be exported.\n" msgstr "%1% ã¯ã‚¨ã‚¯ã‚¹ãƒãƒ¼ãƒˆå‡ºæ¥ã¾ã›ã‚“。\n" #: ItemManager.cpp:839 msgid "%1% cannot be saved as the %2% format.\n" msgstr "%1% ã¯ãƒ•ã‚©ãƒ¼ãƒžãƒƒãƒˆ%2%ã¨ã—ã¦ä¿å­˜å‡ºæ¥ã¾ã›ã‚“。\n" #: ItemManager.cpp:841 msgid "%1% cannot be exported into the %2% format.\n" msgstr "%1% ã¯ãƒ•ã‚©ãƒ¼ãƒžãƒƒãƒˆ%2%ã¨ã—ã¦ã‚¨ã‚¯ã‚¹ãƒãƒ¼ãƒˆå‡ºæ¥ã¾ã›ã‚“。\n" #: ItemManager.cpp:854 msgid "Save %1% as" msgstr "åå‰ã‚’付ã‘ã¦%1%ã‚’ä¿å­˜" #: ItemManager.cpp:857 MainWindow.cpp:946 ProjectManager.cpp:418 #: SceneView.cpp:1848 msgid "Save" msgstr "ä¿å­˜" #: ItemPropertyView.cpp:234 msgid "Property" msgstr "プロパティ" #: ItemTreeArchiver.cpp:46 msgid "\"%1%\" cannot be stored. Its type is not registered." msgstr "\"%1%\" ã¯ãƒ•ã‚©ãƒ¼ãƒžãƒƒãƒˆãŒä¸æ˜ŽãªãŸã‚読ã¿è¾¼ã‚ã¾ã›ã‚“。" #: ItemTreeArchiver.cpp:53 msgid "Storing %1% \"%2%\"" msgstr "%1% \"%2%\"を書ã出ã—中" #: ItemTreeArchiver.cpp:60 msgid "\"%1%\" cannot be stored." msgstr "\"%1%\" ã¯æ›¸ã出ã—出æ¥ã¾ã›ã‚“。" #: ItemTreeArchiver.cpp:122 msgid "Archive is broken." msgstr "アーカイブãŒå£Šã‚Œã¦ã„ã¾ã™ã€‚" #: ItemTreeArchiver.cpp:133 msgid "Item type %1% of %2% cannot be restored. It's not a registered type." msgstr "%2%ã¯æœªç™»éŒ²ã®ã‚¿ã‚¤ãƒ—%1%ã§ã‚ã‚‹ãŸã‚読ã¿è¾¼ã¿å‡ºæ¥ã¾ã›ã‚“。" #: ItemTreeArchiver.cpp:149 msgid "Restoring %1% \"%2%\"" msgstr "%1% \"%2%\" を読ã¿è¾¼ã¿ä¸­" #: ItemTreeArchiver.cpp:170 msgid "%1% \"%2%\" cannot be restored." msgstr "%1% \"%2%\" ã¯èª­ã¿è¾¼ã¿å‡ºæ¥ã¾ã›ã‚“。" #: ItemTreeView.cpp:205 msgid "Items" msgstr "アイテム" #: ItemTreeView.cpp:252 msgid "Cut" msgstr "カット" #: ItemTreeView.cpp:254 msgid "Copy" msgstr "コピー" #: ItemTreeView.cpp:256 msgid "Copy with Children" msgstr "å°ã‚¢ã‚¤ãƒ†ãƒ ã‚‚å«ã‚ã¦ã‚³ãƒ”ー" #: ItemTreeView.cpp:258 msgid "Paste" msgstr "ペースト" #: Licenses.cpp:8 msgid "" "This program is free software; you can redistribute it and/or modify it " "under the terms of the GNU Lesser General Public License as published by the " "Free Software Foundation; either version 2.1 of the License, or (at your " "option) any later version.\n" "\n" "This package is distributed in the hope that it will be useful, but WITHOUT " "ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or " "FITNESS FOR A PARTICULAR PURPOSE.See the GNU Lesser General Public License " "for more details.\n" msgstr "" #: MainWindow.cpp:292 msgid "Exit" msgstr "終了" #: MainWindow.cpp:297 msgid "View" msgstr "表示" #: MainWindow.cpp:299 msgid "Full Screen" msgstr "フルスクリーン" #: MainWindow.cpp:303 msgid "Layout" msgstr "レイアウト" #: MainWindow.cpp:305 msgid "Save Default Layout" msgstr "レイアウトをデフォルトã¨ã—ã¦ä¿å­˜" #: MainWindow.cpp:307 msgid "Load Default Layout" msgstr "デフォルトレイアウトã®èª­ã¿è¾¼ã¿" #: MainWindow.cpp:309 msgid "Save Layout As" msgstr "レイアウトをåå‰ã‚’付ã‘ã¦ä¿å­˜" #: MainWindow.cpp:311 msgid "Load Layout As" msgstr "レイアウトã®èª­ã¿è¾¼ã¿" #: MainWindow.cpp:314 msgid "Tools" msgstr "ツール" #: MainWindow.cpp:315 msgid "Filters" msgstr "フィルタ" #: MainWindow.cpp:316 msgid "Options" msgstr "オプション" #: MainWindow.cpp:317 msgid "Help" msgstr "ヘルプ" #: MainWindow.cpp:699 msgid "Open a layout" msgstr "レイアウトã®èª­ã¿è¾¼ã¿" #: MainWindow.cpp:706 MainWindow.cpp:950 msgid "Layout files (*.conf)" msgstr "レイアウトファイル (*.conf)" #: MainWindow.cpp:943 msgid "Save a layout" msgstr "レイアウトã®ä¿å­˜" #: MessageView.cpp:192 msgid "Message" msgstr "メッセージ" #: MessageView.cpp:376 msgid "Warning" msgstr "警告" #: MovieGenerator.cpp:71 MovieGenerator.cpp:80 msgid "Movie Generator" msgstr "動画用連番ファイルã®ä½œæˆ" #: MovieGenerator.cpp:92 msgid "Folder" msgstr "フォルダ" #: MovieGenerator.cpp:100 msgid "Basename" msgstr "ベースファイルå" #: MovieGenerator.cpp:109 msgid "Begin" msgstr "開始時刻" #: MovieGenerator.cpp:116 msgid "End" msgstr "終了時刻" #: MovieGenerator.cpp:135 msgid "Image size" msgstr "ç”»åƒã‚µã‚¤ã‚º" #: MovieGenerator.cpp:153 msgid "Generate" msgstr "生æˆ" #: MovieGenerator.cpp:154 msgid "Cancel / Stop" msgstr "キャンセル / åœæ­¢" #: MovieGenerator.cpp:234 msgid "%1% is not a directory." msgstr "%1% ã¯ãƒ‡ã‚£ãƒ¬ã‚¯ãƒˆãƒªã§ã¯ã‚ã‚Šã¾ã›ã‚“。" #: MultiAffine3SeqItem.cpp:15 msgid "MultiAffine3SeqItem" msgstr "" #: MultiAffine3SeqItem.cpp:18 msgid "Number of Affine3 values in a frame" msgstr "å„フレームã‚ãŸã‚Šã®Affine3行列ã®æ•°" #: MultiSeqItem.cpp:64 MultiSeqItemCreationPanel.cpp:46 Vector3SeqItem.cpp:98 msgid "Frame rate" msgstr "フレームレート:" #: MultiSeqItem.cpp:65 msgid "Num parts" msgstr "パート数" #: MultiSeqItem.cpp:66 msgid "Num frames" msgstr "フレーム数" #: MultiSeqItem.cpp:67 MultiSeqItemCreationPanel.cpp:39 Vector3SeqItem.cpp:100 msgid "Time length" msgstr "時間長" #: MultiSeqItem.cpp:68 Vector3SeqItem.cpp:101 msgid "Time step" msgstr "タイムステップ" #: MultiValueSeqItem.cpp:38 msgid "MultiValueSeqItem" msgstr "" #: MultiValueSeqItem.cpp:41 msgid "Number of values in a frame" msgstr "å„フレームã‚ãŸã‚Šã®æ•°å€¤ã®æ•°" #: MultiValueSeqItem.cpp:44 msgid "Plain format of a multi value sequence" msgstr "複数数値列ã®ãƒ—レインフォーマット" #: PluginManager.cpp:316 msgid "Plugin file \"%1%\" has already been activated." msgstr "プラグインファイル\"%1%\"ã¯æ—¢ã«èª­ã¿è¾¼ã¾ã‚Œã¦ã„ã¾ã™ã€‚" #: PluginManager.cpp:319 msgid "Loading plugin file \"%1%\"" msgstr "プラグインファイル\"%1%\"を読ã¿è¾¼ã¿ä¸­" #: PluginManager.cpp:331 msgid "The plugin entry function \"getChoreonoidPlugin\" is not found.\n" msgstr "プラグインエントリ関数 \"getChoreonoidPlugin\" ãŒè¦‹ã¤ã‹ã‚Šã¾ã›ã‚“。\n" #: PluginManager.cpp:340 msgid "The plugin object cannot be created." msgstr "プラグインオブジェクトãŒç”Ÿæˆå‡ºæ¥ã¾ã›ã‚“。" #: PluginManager.cpp:363 msgid "The plugin conflicts with plugin \"%1%\"." msgstr "ã“ã®ãƒ—ラグインã¯\"%1%\"ã¨ç«¶åˆã—ã¦ã„ã¾ã™ã€‚" #: PluginManager.cpp:380 msgid "The plugin object cannot be intialized." msgstr "プラグインオブジェクトã®åˆæœŸåŒ–ãŒå‡ºæ¥ã¾ã›ã‚“。" #: PluginManager.cpp:394 msgid "About Plugins" msgstr "プラグインã«ã¤ã„ã¦" #: PluginManager.cpp:395 PluginManager.cpp:429 msgid "About %1% Plugin" msgstr "%1%プラグインã«ã¤ã„ã¦" #: PluginManager.cpp:407 msgid "%1%-plugin has been activated." msgstr "%1%プラグインãŒèª­ã¿è¾¼ã¾ã‚Œã¾ã—ãŸã€‚" #: PluginManager.cpp:416 msgid "Loading the plugin failed." msgstr "プラグインã®èª­ã¿è¾¼ã¿ãŒå‡ºæ¥ã¾ã›ã‚“ã§ã—ãŸã€‚" #: PluginManager.cpp:498 msgid "" "Plugin %1% cannot be finalized because its dependent(s) cannot be finalized." msgstr "" #: PluginManager.cpp:507 #, fuzzy msgid "Plugin %1% cannot be finalized." msgstr "プラグイン %1% ã®çµ‚了処ç†ã«å•é¡ŒãŒã‚ã‚Šã¾ã™ã€‚" #: PluginManager.cpp:540 msgid "Plugin dll %1% has been unloaded." msgstr "プラグイン %1% ã®DLLãŒè§£æ”¾ã•ã‚Œã¾ã—ãŸã€‚" #: ProjectManager.cpp:113 msgid "Open Project" msgstr "プロジェクトã®èª­ã¿è¾¼ã¿" #: ProjectManager.cpp:115 msgid "Save Project" msgstr "プロジェクトã®ä¿å­˜" #: ProjectManager.cpp:117 msgid "Save Project As" msgstr "åå‰ã‚’付ã‘ã¦ãƒ—ロジェクトをä¿å­˜" #: ProjectManager.cpp:120 ProjectPathSetEditor.cpp:80 msgid "Project File Options" msgstr "プロジェクトファイルオプション" #: ProjectManager.cpp:120 msgid "Perspective" msgstr "レイアウト" #: ProjectManager.cpp:179 msgid "Loading project file \"%1%\" ..." msgstr "プロジェクトファイル\"%1%\"を読ã¿è¾¼ã¿ä¸­â€¦" #: ProjectManager.cpp:190 msgid "The project file is empty.\n" msgstr "空ã®ãƒ—ロジェクトファイルã§ã™ã€‚\n" #: ProjectManager.cpp:252 msgid "Project \"%1%\" has successfully been loaded." msgstr "プロジェクト \"%1%\" ã®èª­ã¿è¾¼ã¿ã«æˆåŠŸã—ã¾ã—ãŸã€‚" #: ProjectManager.cpp:255 msgid "Project \"%1%\" cannot be loaded." msgstr "プロジェクト \"%1%\" ã¯èª­ã¿è¾¼ã‚ã¾ã›ã‚“ã§ã—ãŸã€‚" #: ProjectManager.cpp:302 msgid "Saving a project to \"%1%\" ...\n" msgstr "プロジェクトを\"%1%\"ã«ä¿å­˜ä¸­â€¦\n" #: ProjectManager.cpp:352 msgid "Saving a project file has been finished.\n" msgstr "プロジェクトファイルã®ä¿å­˜ãŒçµ‚了ã—ã¾ã—ãŸã€‚\n" #: ProjectManager.cpp:355 msgid "Saving a project file failed.\n" msgstr "プロジェクトファイルãŒä¿å­˜ã§ãã¾ã›ã‚“ã§ã—ãŸã€‚\n" #: ProjectManager.cpp:391 msgid "Open a choreonoid project file" msgstr "Choreonoid プロジェクトファイルã®èª­ã¿è¾¼ã¿" #: ProjectManager.cpp:398 ProjectManager.cpp:422 msgid "Project files (*.cnoid)" msgstr "プロジェクトファイル (*.cnoid)" #: ProjectManager.cpp:415 msgid "Save a choreonoid project file" msgstr "Choreonoid プロジェクトファイルã®ä¿å­˜" #: ProjectPathSetEditor.cpp:81 msgid "Edit Path Set" msgstr "パスセットã®ç·¨é›†" #: ProjectPathSetEditor.cpp:91 msgid "Project File Path Set" msgstr "プロジェクトファイルã®ãƒ‘スセット" #: ProjectPathSetEditor.cpp:99 ProjectPathSetEditor.cpp:109 msgid "Variable" msgstr "変数" #: ProjectPathSetEditor.cpp:100 msgid "Path List" msgstr "パス" #: ProjectPathSetEditor.cpp:113 msgid "Append" msgstr "追加" #: ProjectPathSetEditor.cpp:118 msgid "&Apply" msgstr "é©ç”¨(&A)" #: RootItem.cpp:44 msgid "RootItem" msgstr "ルートアイテム" #: SceneItem.cpp:54 SceneItem.cpp:156 msgid "Scene file" msgstr "シーンファイル" #: SceneView.cpp:255 msgid "SceneView Setup" msgstr "シーンビューã®è¨­å®š" #: SceneView.cpp:259 msgid "Floor Grid" msgstr "床グリッド" #: SceneView.cpp:262 msgid "Span" msgstr "é•·ã•" #: SceneView.cpp:272 msgid "Interval" msgstr "é–“éš”" #: SceneView.cpp:286 msgid "Hi-priority rendering mode" msgstr "高優先度æ画モード" #: SceneView.cpp:350 msgid "Scene" msgstr "シーン" #: SceneView.cpp:455 msgid "Scene View" msgstr "シーンビュー" #: SceneView.cpp:457 msgid "Coordinate axes" msgstr "座標軸" #: SceneView.cpp:461 msgid "Scribe mode" msgstr "Scribe モード" #: SceneView.cpp:464 msgid "Cartoon mode on/off" msgstr "アニメ調モードã®ã‚ªãƒ³ï¼ã‚ªãƒ•" #: SceneView.cpp:467 msgid "toggle shadow on/off" msgstr "å½±ã®ã‚ªãƒ³ï¼ã‚ªãƒ•" #: SceneView.cpp:470 msgid "Over paint mode" msgstr "上æãモード" #: SceneView.cpp:477 msgid "SceneBar" msgstr "シーンãƒãƒ¼" #: SceneView.cpp:479 msgid "Switch to the edit mode" msgstr "編集モードã¸ã®åˆ‡ã‚Šæ›¿ãˆ" #: SceneView.cpp:483 msgid "Move the camera to look at the objects" msgstr "å…¨ã¦ã®ç‰©ä½“ãŒè¦‹ãˆã‚‹ã‚ˆã†ã«è¦–点を移動" #: SceneView.cpp:486 msgid "Toggle the perspective / orthogonal view mode" msgstr "é€è¦–法射影/正射影ã®åˆ‡ã‚Šæ›¿ãˆ" #: SceneView.cpp:490 msgid "Show the floor grid" msgstr "床グリッド線ã®è¡¨ç¤º" #: SceneView.cpp:494 msgid "Toggle the collision visualization" msgstr "干渉線ã®è¡¨ç¤º" #: SceneView.cpp:499 msgid "Toggle the wireframe mode" msgstr "ワイヤフレーム表示" #: SceneView.cpp:502 msgid "Capture a screen image and save it into a file" msgstr "シーンを画åƒãƒ•ã‚¡ã‚¤ãƒ«ã«ä¿å­˜" #: SceneView.cpp:505 msgid "Open the dialog to setup SceneView" msgstr "シーンビューã®è¨­å®šãƒ€ã‚¤ã‚¢ãƒ­ã‚°ã‚’é–‹ã" #: SceneView.cpp:1325 #, c-format msgid "Position = (%.3f %.3f %.3f)" msgstr "ä½ç½® = (%.3f %.3f %.3f)" #: SceneView.cpp:1420 msgid "Edit Mode" msgstr "編集モード" #: SceneView.cpp:1431 msgid "View Mode" msgstr "閲覧モード" #: SceneView.cpp:1845 msgid "Save an image of the scene view" msgstr "シーンビューã®ç”»åƒã‚’ä¿å­˜" #: TimeBar.cpp:64 msgid "Time Bar Setup" msgstr "タイムãƒãƒ¼ã®è¨­å®š" #: TimeBar.cpp:70 msgid "Internal frame rate" msgstr "内部フレームレート" #: TimeBar.cpp:79 msgid "Playback frame rate" msgstr "å†ç”Ÿãƒ•ãƒ¬ãƒ¼ãƒ ãƒ¬ãƒ¼ãƒˆ" #: TimeBar.cpp:89 msgid "Playback speed scale" msgstr "å†ç”Ÿã‚¹ãƒ”ードå€çŽ‡" #: TimeBar.cpp:138 msgid "&OK" msgstr "&OK" #: TimeBar.cpp:248 msgid "TimeBar" msgstr "タイムãƒãƒ¼" #: TimeBar.cpp:273 msgid "Start animation" msgstr "アニメーションã®é–‹å§‹" #: TimeBar.cpp:276 TimeBar.cpp:608 msgid "Resume animation" msgstr "アニメーションã®å†ç”Ÿãƒ»åœæ­¢" #: TimeBar.cpp:280 msgid "Refresh state at the current time" msgstr "ç¾åœ¨æ™‚刻ã§çŠ¶æ…‹ã‚’æ›´æ–°" #: TimeBar.cpp:306 msgid "Open the setup dialog" msgstr "設定ダイアログを開ãã¾ã™" #: TimeBar.cpp:584 msgid "Stop animation" msgstr "アニメーションã®åœæ­¢" #: Vector3SeqItem.cpp:15 msgid "Vector3SeqItem" msgstr "３次元ベクタ列アイテム" #: Vector3SeqItem.cpp:99 msgid "Number of frames" msgstr "フレーム数" #~ msgid "The system might not work correctly." #~ msgstr "システムãŒã†ã¾ã動作ã—ãªã„ã‹ã‚‚ã—ã‚Œã¾ã›ã‚“。" choreonoid-1.1.0+dfsg/src/Base/po/messages.pot000066400000000000000000000355741207742442300212160ustar00rootroot00000000000000# SOME DESCRIPTIVE TITLE. # Copyright (C) YEAR THE PACKAGE'S COPYRIGHT HOLDER # This file is distributed under the same license as the PACKAGE package. # FIRST AUTHOR , YEAR. # #, fuzzy msgid "" msgstr "" "Project-Id-Version: PACKAGE VERSION\n" "Report-Msgid-Bugs-To: \n" "POT-Creation-Date: 2011-12-28 17:54+0000\n" "PO-Revision-Date: YEAR-MO-DA HO:MI+ZONE\n" "Last-Translator: FULL NAME \n" "Language-Team: LANGUAGE \n" "Language: \n" "MIME-Version: 1.0\n" "Content-Type: text/plain; charset=CHARSET\n" "Content-Transfer-Encoding: 8bit\n" #: App.cpp:158 App.cpp:317 msgid "About Choreonoid Framework" msgstr "" #: App.cpp:320 msgid "Choreonoid Framework Version %1\n" msgstr "" #: App.cpp:321 msgid "" "\n" "This program has been developed by Shin'ichiro Nakaoka and Choreonoid " "Development Team, AIST, and is distributed as a part of the Choreonoid " "package.\n" "\n" msgstr "" #: App.cpp:326 msgid "" "The source and some binary packages of this program also include the " "following thirdparty libraries:\n" "\n" "* Eigen (http://eigen.tuxfamily.org/)\n" "* IJG JPEG Library (http://www.ijg.org/)\n" "* libpng (http://www.libpng.org/pub/png/libpng.html)\n" "* LibYAML (http://pyyaml.org/wiki/LibYAML)\n" "* zlib (http://zlib.net/)\n" "\n" "These libraries are used and redistributed under the terms of their " "licenses. Please see the corresponding directories in the source package or " "their official web sites to see the details of their licenses.\n" "\n" "This program also depends on a lot of other external libraries, which are " "linked into this program during the execution. Some binary packages of this " "program may include the binaries of the following libraries:\n" "\n" "* Qt (http://qt.nokia.com/)\n" "* OpenSceneGraph (http://www.openscenegraph.org)\n" "* Boost C++ Libraries (http://www.boost.org/) \n" "\n" "Please see their official web sites to see the details of their licenses.\n" msgstr "" #: Archive.cpp:87 msgid "Warning: ${%1} of \"%2\" cannot be expanded !" msgstr "" #: DescriptionDialog.cpp:28 SceneView.cpp:294 msgid "&Ok" msgstr "" #: FileBar.cpp:34 msgid "FileBar" msgstr "" #: FileBar.cpp:36 msgid "Save the project" msgstr "" #: FolderItem.cpp:60 msgid "FolderItem" msgstr "" #: GraphBar.cpp:88 msgid "GraphBar" msgstr "" #: GraphBar.cpp:97 msgid "Plot trajectories of the target data on the graph view" msgstr "" #: GraphBar.cpp:103 msgid "Plot velocity trajectories" msgstr "" #: GraphBar.cpp:108 msgid "Plot acceleration trajectories" msgstr "" #: GraphBar.cpp:113 msgid "Show options of the graph view" msgstr "" #: GraphBar.cpp:130 msgid "Limit" msgstr "" #: GraphBar.cpp:131 msgid "" "Show the lower / upper limit values on the graph. Trajectory edit is also " "limited within the values." msgstr "" #: GraphBar.cpp:138 msgid "LW" msgstr "" #: GraphBar.cpp:147 msgid "Grid" msgstr "" #: GraphBar.cpp:147 msgid "Show a grid on the graph background" msgstr "" #: GraphBar.cpp:164 msgid "Ruler" msgstr "" #: GraphBar.cpp:164 msgid "Show rulers" msgstr "" #: GraphBar.cpp:173 MainWindow.cpp:295 msgid "Edit" msgstr "" #: GraphBar.cpp:173 msgid "Edit mode on/off" msgstr "" #: GraphBar.cpp:179 msgid "Free" msgstr "" #: GraphBar.cpp:179 msgid "Free line edit mode" msgstr "" #: GraphBar.cpp:186 msgid "Line" msgstr "" #: GraphBar.cpp:186 msgid "Line edit mode" msgstr "" #: GraphBar.cpp:192 msgid "step" msgstr "" #: GraphBar.cpp:203 msgid "offset" msgstr "" #: GraphBar.cpp:214 msgid "CP" msgstr "" #: GraphBar.cpp:214 msgid "Highlighting control points" msgstr "" #: GraphBar.cpp:222 msgid "Sync" msgstr "" #: GraphBar.cpp:222 msgid "Cursor movement on a graph updates the time bar position" msgstr "" #: GraphBar.cpp:232 msgid "off" msgstr "" #: GraphBar.cpp:233 msgid "cont." msgstr "" #: GraphBar.cpp:234 msgid "page" msgstr "" #: GraphWidget.cpp:1136 #, c-format msgid "Graph: Position = (%1$.5f, %2$.5f)" msgstr "" #: Item.cpp:630 MultiSeqItemCreationPanel.cpp:27 msgid "Name" msgstr "" #: Item.cpp:634 msgid "Class" msgstr "" #: Item.cpp:639 MainWindow.cpp:292 msgid "File" msgstr "" #: Item.cpp:642 msgid "Refs" msgstr "" #: Item.cpp:643 msgid "Sub item ?" msgstr "" #: ItemManager.cpp:37 msgid "Name:" msgstr "" #: ItemManager.cpp:91 msgid "New ..." msgstr "" #: ItemManager.cpp:94 msgid "Open Item" msgstr "" #: ItemManager.cpp:96 msgid "Open ..." msgstr "" #: ItemManager.cpp:98 msgid "Reload Selected Items" msgstr "" #: ItemManager.cpp:103 msgid "Save Selected Items" msgstr "" #: ItemManager.cpp:105 msgid "Save Selected Items As" msgstr "" #: ItemManager.cpp:107 msgid "Save All Items" msgstr "" #: ItemManager.cpp:112 msgid "Import ..." msgstr "" #: ItemManager.cpp:114 msgid "Export Selected Items" msgstr "" #: ItemManager.cpp:348 msgid "Item$" msgstr "" #: ItemManager.cpp:351 msgid "Create New %1" msgstr "" #: ItemManager.cpp:392 msgid "&Create" msgstr "" #: ItemManager.cpp:395 ProjectPathSetEditor.cpp:120 msgid "&Cancel" msgstr "" #: ItemManager.cpp:618 msgid "\"%1%\" cannot be loaded because the file format is unknown." msgstr "" #: ItemManager.cpp:621 msgid "\"%1%\" cannot be loaded because file format \"%2%\" is unknown." msgstr "" #: ItemManager.cpp:643 ItemManager.cpp:644 msgid "Loading %1% \"%2%\"" msgstr "" #: ItemManager.cpp:661 ItemManager.cpp:823 msgid " -> ok!\n" msgstr "" #: ItemManager.cpp:661 ItemManager.cpp:823 msgid " -> failed.\n" msgstr "" #: ItemManager.cpp:679 msgid "Load %1%" msgstr "" #: ItemManager.cpp:682 MainWindow.cpp:702 MovieGenerator.cpp:97 #: ProjectManager.cpp:394 msgid "Open" msgstr "" #: ItemManager.cpp:683 ItemManager.cpp:858 MainWindow.cpp:703 #: MainWindow.cpp:947 ProjectManager.cpp:395 ProjectManager.cpp:419 #: SceneView.cpp:1849 msgid "Cancel" msgstr "" #: ItemManager.cpp:700 MainWindow.cpp:707 MainWindow.cpp:951 #: ProjectManager.cpp:399 ProjectManager.cpp:423 SceneView.cpp:1852 msgid "Any files (*)" msgstr "" #: ItemManager.cpp:804 msgid "Saving %1% to \"%2%\"" msgstr "" #: ItemManager.cpp:806 msgid "Exporting %1% into \"%2%\"" msgstr "" #: ItemManager.cpp:833 msgid "%1% cannot be saved.\n" msgstr "" #: ItemManager.cpp:835 msgid "%1% cannot be exported.\n" msgstr "" #: ItemManager.cpp:839 msgid "%1% cannot be saved as the %2% format.\n" msgstr "" #: ItemManager.cpp:841 msgid "%1% cannot be exported into the %2% format.\n" msgstr "" #: ItemManager.cpp:854 msgid "Save %1% as" msgstr "" #: ItemManager.cpp:857 MainWindow.cpp:946 ProjectManager.cpp:418 #: SceneView.cpp:1848 msgid "Save" msgstr "" #: ItemPropertyView.cpp:234 msgid "Property" msgstr "" #: ItemTreeArchiver.cpp:46 msgid "\"%1%\" cannot be stored. Its type is not registered." msgstr "" #: ItemTreeArchiver.cpp:53 msgid "Storing %1% \"%2%\"" msgstr "" #: ItemTreeArchiver.cpp:60 msgid "\"%1%\" cannot be stored." msgstr "" #: ItemTreeArchiver.cpp:122 msgid "Archive is broken." msgstr "" #: ItemTreeArchiver.cpp:133 msgid "Item type %1% of %2% cannot be restored. It's not a registered type." msgstr "" #: ItemTreeArchiver.cpp:149 msgid "Restoring %1% \"%2%\"" msgstr "" #: ItemTreeArchiver.cpp:170 msgid "%1% \"%2%\" cannot be restored." msgstr "" #: ItemTreeView.cpp:205 msgid "Items" msgstr "" #: ItemTreeView.cpp:252 msgid "Cut" msgstr "" #: ItemTreeView.cpp:254 msgid "Copy" msgstr "" #: ItemTreeView.cpp:256 msgid "Copy with Children" msgstr "" #: ItemTreeView.cpp:258 msgid "Paste" msgstr "" #: Licenses.cpp:8 msgid "" "This program is free software; you can redistribute it and/or modify it " "under the terms of the GNU Lesser General Public License as published by the " "Free Software Foundation; either version 2.1 of the License, or (at your " "option) any later version.\n" "\n" "This package is distributed in the hope that it will be useful, but WITHOUT " "ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or " "FITNESS FOR A PARTICULAR PURPOSE.See the GNU Lesser General Public License " "for more details.\n" msgstr "" #: MainWindow.cpp:292 msgid "Exit" msgstr "" #: MainWindow.cpp:297 msgid "View" msgstr "" #: MainWindow.cpp:299 msgid "Full Screen" msgstr "" #: MainWindow.cpp:303 msgid "Layout" msgstr "" #: MainWindow.cpp:305 msgid "Save Default Layout" msgstr "" #: MainWindow.cpp:307 msgid "Load Default Layout" msgstr "" #: MainWindow.cpp:309 msgid "Save Layout As" msgstr "" #: MainWindow.cpp:311 msgid "Load Layout As" msgstr "" #: MainWindow.cpp:314 msgid "Tools" msgstr "" #: MainWindow.cpp:315 msgid "Filters" msgstr "" #: MainWindow.cpp:316 msgid "Options" msgstr "" #: MainWindow.cpp:317 msgid "Help" msgstr "" #: MainWindow.cpp:699 msgid "Open a layout" msgstr "" #: MainWindow.cpp:706 MainWindow.cpp:950 msgid "Layout files (*.conf)" msgstr "" #: MainWindow.cpp:943 msgid "Save a layout" msgstr "" #: MessageView.cpp:192 msgid "Message" msgstr "" #: MessageView.cpp:376 msgid "Warning" msgstr "" #: MovieGenerator.cpp:71 MovieGenerator.cpp:80 msgid "Movie Generator" msgstr "" #: MovieGenerator.cpp:92 msgid "Folder" msgstr "" #: MovieGenerator.cpp:100 msgid "Basename" msgstr "" #: MovieGenerator.cpp:109 msgid "Begin" msgstr "" #: MovieGenerator.cpp:116 msgid "End" msgstr "" #: MovieGenerator.cpp:135 msgid "Image size" msgstr "" #: MovieGenerator.cpp:153 msgid "Generate" msgstr "" #: MovieGenerator.cpp:154 msgid "Cancel / Stop" msgstr "" #: MovieGenerator.cpp:234 msgid "%1% is not a directory." msgstr "" #: MultiAffine3SeqItem.cpp:15 msgid "MultiAffine3SeqItem" msgstr "" #: MultiAffine3SeqItem.cpp:18 msgid "Number of Affine3 values in a frame" msgstr "" #: MultiSeqItem.cpp:64 MultiSeqItemCreationPanel.cpp:46 Vector3SeqItem.cpp:98 msgid "Frame rate" msgstr "" #: MultiSeqItem.cpp:65 msgid "Num parts" msgstr "" #: MultiSeqItem.cpp:66 msgid "Num frames" msgstr "" #: MultiSeqItem.cpp:67 MultiSeqItemCreationPanel.cpp:39 Vector3SeqItem.cpp:100 msgid "Time length" msgstr "" #: MultiSeqItem.cpp:68 Vector3SeqItem.cpp:101 msgid "Time step" msgstr "" #: MultiValueSeqItem.cpp:38 msgid "MultiValueSeqItem" msgstr "" #: MultiValueSeqItem.cpp:41 msgid "Number of values in a frame" msgstr "" #: MultiValueSeqItem.cpp:44 msgid "Plain format of a multi value sequence" msgstr "" #: PluginManager.cpp:316 msgid "Plugin file \"%1%\" has already been activated." msgstr "" #: PluginManager.cpp:319 msgid "Loading plugin file \"%1%\"" msgstr "" #: PluginManager.cpp:331 msgid "The plugin entry function \"getChoreonoidPlugin\" is not found.\n" msgstr "" #: PluginManager.cpp:340 msgid "The plugin object cannot be created." msgstr "" #: PluginManager.cpp:363 msgid "The plugin conflicts with plugin \"%1%\"." msgstr "" #: PluginManager.cpp:380 msgid "The plugin object cannot be intialized." msgstr "" #: PluginManager.cpp:394 msgid "About Plugins" msgstr "" #: PluginManager.cpp:395 PluginManager.cpp:429 msgid "About %1% Plugin" msgstr "" #: PluginManager.cpp:407 msgid "%1%-plugin has been activated." msgstr "" #: PluginManager.cpp:416 msgid "Loading the plugin failed." msgstr "" #: PluginManager.cpp:498 msgid "" "Plugin %1% cannot be finalized because its dependent(s) cannot be finalized." msgstr "" #: PluginManager.cpp:507 msgid "Plugin %1% cannot be finalized." msgstr "" #: PluginManager.cpp:540 msgid "Plugin dll %1% has been unloaded." msgstr "" #: ProjectManager.cpp:113 msgid "Open Project" msgstr "" #: ProjectManager.cpp:115 msgid "Save Project" msgstr "" #: ProjectManager.cpp:117 msgid "Save Project As" msgstr "" #: ProjectManager.cpp:120 ProjectPathSetEditor.cpp:80 msgid "Project File Options" msgstr "" #: ProjectManager.cpp:120 msgid "Perspective" msgstr "" #: ProjectManager.cpp:179 msgid "Loading project file \"%1%\" ..." msgstr "" #: ProjectManager.cpp:190 msgid "The project file is empty.\n" msgstr "" #: ProjectManager.cpp:252 msgid "Project \"%1%\" has successfully been loaded." msgstr "" #: ProjectManager.cpp:255 msgid "Project \"%1%\" cannot be loaded." msgstr "" #: ProjectManager.cpp:302 msgid "Saving a project to \"%1%\" ...\n" msgstr "" #: ProjectManager.cpp:352 msgid "Saving a project file has been finished.\n" msgstr "" #: ProjectManager.cpp:355 msgid "Saving a project file failed.\n" msgstr "" #: ProjectManager.cpp:391 msgid "Open a choreonoid project file" msgstr "" #: ProjectManager.cpp:398 ProjectManager.cpp:422 msgid "Project files (*.cnoid)" msgstr "" #: ProjectManager.cpp:415 msgid "Save a choreonoid project file" msgstr "" #: ProjectPathSetEditor.cpp:81 msgid "Edit Path Set" msgstr "" #: ProjectPathSetEditor.cpp:91 msgid "Project File Path Set" msgstr "" #: ProjectPathSetEditor.cpp:99 ProjectPathSetEditor.cpp:109 msgid "Variable" msgstr "" #: ProjectPathSetEditor.cpp:100 msgid "Path List" msgstr "" #: ProjectPathSetEditor.cpp:113 msgid "Append" msgstr "" #: ProjectPathSetEditor.cpp:118 msgid "&Apply" msgstr "" #: RootItem.cpp:44 msgid "RootItem" msgstr "" #: SceneItem.cpp:54 SceneItem.cpp:156 msgid "Scene file" msgstr "" #: SceneView.cpp:255 msgid "SceneView Setup" msgstr "" #: SceneView.cpp:259 msgid "Floor Grid" msgstr "" #: SceneView.cpp:262 msgid "Span" msgstr "" #: SceneView.cpp:272 msgid "Interval" msgstr "" #: SceneView.cpp:286 msgid "Hi-priority rendering mode" msgstr "" #: SceneView.cpp:350 msgid "Scene" msgstr "" #: SceneView.cpp:455 msgid "Scene View" msgstr "" #: SceneView.cpp:457 msgid "Coordinate axes" msgstr "" #: SceneView.cpp:461 msgid "Scribe mode" msgstr "" #: SceneView.cpp:464 msgid "Cartoon mode on/off" msgstr "" #: SceneView.cpp:467 msgid "toggle shadow on/off" msgstr "" #: SceneView.cpp:470 msgid "Over paint mode" msgstr "" #: SceneView.cpp:477 msgid "SceneBar" msgstr "" #: SceneView.cpp:479 msgid "Switch to the edit mode" msgstr "" #: SceneView.cpp:483 msgid "Move the camera to look at the objects" msgstr "" #: SceneView.cpp:486 msgid "Toggle the perspective / orthogonal view mode" msgstr "" #: SceneView.cpp:490 msgid "Show the floor grid" msgstr "" #: SceneView.cpp:494 msgid "Toggle the collision visualization" msgstr "" #: SceneView.cpp:499 msgid "Toggle the wireframe mode" msgstr "" #: SceneView.cpp:502 msgid "Capture a screen image and save it into a file" msgstr "" #: SceneView.cpp:505 msgid "Open the dialog to setup SceneView" msgstr "" #: SceneView.cpp:1325 #, c-format msgid "Position = (%.3f %.3f %.3f)" msgstr "" #: SceneView.cpp:1420 msgid "Edit Mode" msgstr "" #: SceneView.cpp:1431 msgid "View Mode" msgstr "" #: SceneView.cpp:1845 msgid "Save an image of the scene view" msgstr "" #: TimeBar.cpp:64 msgid "Time Bar Setup" msgstr "" #: TimeBar.cpp:70 msgid "Internal frame rate" msgstr "" #: TimeBar.cpp:79 msgid "Playback frame rate" msgstr "" #: TimeBar.cpp:89 msgid "Playback speed scale" msgstr "" #: TimeBar.cpp:138 msgid "&OK" msgstr "" #: TimeBar.cpp:248 msgid "TimeBar" msgstr "" #: TimeBar.cpp:273 msgid "Start animation" msgstr "" #: TimeBar.cpp:276 TimeBar.cpp:608 msgid "Resume animation" msgstr "" #: TimeBar.cpp:280 msgid "Refresh state at the current time" msgstr "" #: TimeBar.cpp:306 msgid "Open the setup dialog" msgstr "" #: TimeBar.cpp:584 msgid "Stop animation" msgstr "" #: Vector3SeqItem.cpp:15 msgid "Vector3SeqItem" msgstr "" #: Vector3SeqItem.cpp:99 msgid "Number of frames" msgstr "" choreonoid-1.1.0+dfsg/src/Body/000077500000000000000000000000001207742442300162525ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Body/Body.cpp000066400000000000000000000434321207742442300176610ustar00rootroot00000000000000/** \file \brief The implementation of the body class \author Shin'ichiro Nakaoka */ #include "Body.h" #include "BodyCustomizerInterface.h" #include "Link.h" #include "LinkGroup.h" #include "JointPath.h" #include "Sensor.h" #include "CompositeIK.h" #include #include #include using namespace cnoid; using namespace std; static const bool PUT_DEBUG_MESSAGE = true; static bool pluginsInDefaultDirectoriesLoaded = false; #ifndef uint typedef unsigned int uint; #endif namespace cnoid { class CustomizedJointPath : public JointPath { Body* body; int ikTypeId; bool isCustomizedIkPathReversed; virtual void onJointPathUpdated(); public: CustomizedJointPath(Body* body, Link* baseLink, Link* targetLink); virtual ~CustomizedJointPath(); virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R); virtual bool hasAnalyticalIK() const; }; } Body::~Body() { if(customizerHandle){ customizerInterface->destroy(customizerHandle); } delete rootLink_; // delete sensors for(int sensorType =0; sensorType < numSensorTypes(); ++sensorType){ int n = numSensors(sensorType); for(int i=0; i < n; ++i){ Sensor* s = sensor(sensorType, i); if(s){ Sensor::destroy(s); } } } } void Body::initialize() { rootLink_ = 0; customizerHandle = 0; customizerInterface = 0; bodyHandleEntity.body = this; bodyHandle = &bodyHandleEntity; } Body::Body() : allSensors(Sensor::NUM_SENSOR_TYPES) { initialize(); rootLink_ = new Link; rootLink_->body = this; lastCM_.setZero(); info_ = new YamlMapping(); defaultRootPosition.setZero(); defaultRootAttitude.setIdentity(); } Body::Body(const Body& org) : name_(org.name_), modelName_(org.modelName_), allSensors(Sensor::NUM_SENSOR_TYPES), info_(org.info_) { initialize(); lastCM_ = org.lastCM_; setRootLink(new Link(*org.rootLink())); defaultRootPosition = org.defaultRootPosition; defaultRootAttitude = org.defaultRootAttitude; // copy sensors std::map linkToIndexMap; for(int i=0; i < org.linkTraverse_.numLinks(); ++i){ Link* lnk = org.linkTraverse_[i]; linkToIndexMap[lnk] = i; } int n = org.numSensorTypes(); for(int sensorType = 0; sensorType < n; ++sensorType){ for(int i=0; i < org.numSensors(sensorType); ++i){ Sensor* orgSensor = org.sensor(sensorType, i); int linkIndex = linkToIndexMap[orgSensor->link]; Link* newLink = linkTraverse_[linkIndex]; Sensor* cloneSensor = createSensor(newLink, sensorType, orgSensor->id, orgSensor->name); *cloneSensor = *orgSensor; } } // do deep copy of linkConnections for(size_t i=0; i < org.linkConnections.size(); ++i){ const LinkConnection& orgConnection = org.linkConnections[i]; LinkConnection connection(orgConnection); for(int j=0; j < 2; ++j){ connection.link[j] = link(orgConnection.link[j]->index); } } if(org.customizerInterface){ installCustomizer(org.customizerInterface); } } BodyPtr Body::duplicate() const { return new Body(*this); } void Body::setRootLink(Link* link) { if(rootLink_){ delete rootLink_; } rootLink_ = link; updateLinkTree(); } void Body::setDefaultRootPosition(const Vector3& p, const Matrix3& R) { defaultRootPosition = p; defaultRootAttitude = R; } void Body::getDefaultRootPosition(Vector3& out_p, Matrix3& out_R) { out_p = defaultRootPosition; out_R = defaultRootAttitude; } Link* Body::createEmptyJoint(int jointId) { Link* empty = new Link; empty->body = this; empty->jointId = jointId; empty->p.setZero(); empty->R.setIdentity(); empty->v.setZero(); empty->w.setZero(); empty->dv.setZero(); empty->dw.setZero(); empty->q = 0.0; empty->dq = 0.0; empty->ddq = 0.0; empty->u = 0.0; empty->a.setZero(); empty->d.setZero(); empty->b.setZero(); empty->Rs.setIdentity(); empty->m = 0.0; empty->I.setZero(); empty->c.setZero(); empty->wc.setZero(); empty->vo.setZero(); empty->dvo.setZero(); empty->fext.setZero(); empty->tauext.setZero(); empty->Jm2 = 0.0; empty->ulimit = 0.0; empty->llimit = 0.0; empty->uvlimit = 0.0; empty->lvlimit = 0.0; empty->defaultJointValue = 0.0; empty->Ir = 0.0; empty->gearRatio = 1.0; return empty; } void Body::updateLinkTree() { nameToLinkMap.clear(); linkTraverse_.find(rootLink()); int n = linkTraverse_.numLinks(); jointIdToLinkArray.clear(); jointIdToLinkArray.resize(n, 0); int maxJointID = -1; for(int i=0; i < n; ++i){ Link* link = linkTraverse_[i]; link->body = this; link->index = i; nameToLinkMap[link->name()] = link; int id = link->jointId; if(id >= 0){ int size = jointIdToLinkArray.size(); if(id >= size){ jointIdToLinkArray.resize(id + 1, 0); } jointIdToLinkArray[id] = link; if(id > maxJointID){ maxJointID = id; } } } jointIdToLinkArray.resize(maxJointID + 1); for(size_t i=0; i < jointIdToLinkArray.size(); ++i){ if(!jointIdToLinkArray[i]){ jointIdToLinkArray[i] = createEmptyJoint(i); std::cerr << "Warning: Model " << modelName_ << " has empty joint ID in the valid IDs." << std::endl; } } calcTotalMass(); isStaticModel_ = (rootLink_->jointType == Link::FIXED_JOINT && numJoints() == 0); } void Body::initializeConfiguration() { rootLink_->p = defaultRootPosition; rootLink_->setAttitude(defaultRootAttitude); rootLink_->v.setZero(); rootLink_->dv.setZero(); rootLink_->w.setZero(); rootLink_->dw.setZero(); rootLink_->vo.setZero(); rootLink_->dvo.setZero(); int n = linkTraverse_.numLinks(); for(int i=0; i < n; ++i){ Link* link = linkTraverse_[i]; link->u = 0.0; link->q = 0.0; link->dq = 0.0; link->ddq = 0.0; } calcForwardKinematics(true, true); clearExternalForces(); } void Body::resetInfo(YamlMappingPtr info) { info_ = info; doResetInfo(*info); } void Body::doResetInfo(const YamlMapping& info) { linkGroup_ = LinkGroup::create(this, *info.findSequence("linkGroup")); } /** This function returns a link object whose name of Joint node matches a given name. Null is returned when the body has no joint of the given name. */ Link* Body::link(const std::string& name) const { NameToLinkMap::const_iterator p = nameToLinkMap.find(name); return (p != nameToLinkMap.end()) ? p->second : 0; } double Body::calcTotalMass() { totalMass_ = 0.0; int n = linkTraverse_.numLinks(); for(int i=0; i < n; ++i){ totalMass_ += linkTraverse_[i]->m; } return totalMass_; } Vector3 Body::calcCM() { totalMass_ = 0.0; Vector3 mc = Vector3::Zero(); int n = linkTraverse_.numLinks(); for(int i=0; i < n; i++){ Link* link = linkTraverse_[i]; link->wc.noalias() = link->p + link->R * link->c; mc.noalias() += link->m * link->wc; totalMass_ += link->m; } lastCM_ = mc / totalMass_; return lastCM_; } /** assuming Link::v,w is already computed by calcForwardKinematics(true); assuming Link::wc is already computed by calcCM(); */ void Body::calcTotalMomentum(Vector3& out_P, Vector3& out_L) { out_P.setZero(); out_L.setZero(); Vector3 dwc; // Center of mass speed in world frame Vector3 P; // Linear momentum of the link Vector3 L; // Angular momentum with respect to the world frame origin Vector3 Llocal; // Angular momentum with respect to the center of mass of the link int n = linkTraverse_.numLinks(); for(int i=0; i < n; i++){ Link* link = linkTraverse_[i]; dwc = link->v + link->w.cross(link->R * link->c); P = link->m * dwc; //L = cross(link->wc, P) + link->R * link->I * trans(link->R) * link->w; Llocal.noalias() = link->I * link->R.transpose() * link->w; L .noalias() = link->wc.cross(P) + link->R * Llocal; out_P += P; out_L += L; } } void Body::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) { linkTraverse_.calcForwardKinematics(calcVelocity, calcAcceleration); } Sensor* Body::createSensor(Link* link, int sensorType, int id, const std::string& name) { Sensor* sensor = 0; if(sensorType < Sensor::NUM_SENSOR_TYPES && id >= 0){ SensorArray& sensors = allSensors[sensorType]; int n = sensors.size(); if(id >= n){ sensors.resize(id + 1, 0); } sensor = sensors[id]; if(sensor){ std::cerr << "duplicated sensor Id is specified(id = " << id << ", name = " << name << ")" << std::endl; nameToSensorMap.erase(sensor->name); } else { sensor = Sensor::create(sensorType); } if(sensor){ sensor->id = id; sensors[id] = sensor; sensor->link = link; sensor->name = name; nameToSensorMap[name] = sensor; } } return sensor; } void Body::addSensor(Sensor* sensor, int sensorType, int id ){ if(sensorType < Sensor::NUM_SENSOR_TYPES && id >= 0){ SensorArray& sensors = allSensors[sensorType]; int n = sensors.size(); if(id >= n){ sensors.resize(id + 1, 0); } Sensor* sameId = sensors[id]; if(sameId){ std::cerr << "duplicated sensor Id is specified(id = " << id << ", name = " << sensor->name << ")" << std::endl; nameToSensorMap.erase(sameId->name); } sensors[id] = sensor; nameToSensorMap[sensor->name] = sensor; } } void Body::clearSensorValues() { for(int i=0; i < numSensorTypes(); ++i){ for(int j=0; j < numSensors(i); ++j){ sensor(i, j)->clear(); } } } JointPathPtr Body::getJointPath(Link* baseLink, Link* targetLink) { if(customizerInterface && customizerInterface->initializeAnalyticIk){ return JointPathPtr(new CustomizedJointPath(this, baseLink, targetLink)); } else { return JointPathPtr(new JointPath(baseLink, targetLink)); } } InverseKinematicsPtr Body::getDefaultIK(Link* targetLink) { InverseKinematicsPtr ik; const YamlMapping& setupMap = *info_->findMapping("defaultIKsetup"); if(targetLink && setupMap.isValid()){ const YamlSequence& setup = *setupMap.findSequence(targetLink->name()); if(setup.isValid() && !setup.empty()){ Link* baseLink = link(setup[0].toString()); if(baseLink){ if(setup.size() == 1){ ik = getJointPath(baseLink, targetLink); } else { CompositeIKptr compositeIK(new CompositeIK(this, targetLink)); ik = compositeIK; for(int i=0; i < setup.size(); ++i){ Link* baseLink = link(setup[i].toString()); if(baseLink){ if(!compositeIK->addBaseLink(baseLink)){ ik.reset(); break; } } } /* PinDragIKptr pinDragIK(new PinDragIK(this)); pinDragIK->setBaseLink(baseLink); pinDragIK->setTargetLink(targetLink, true); for(int i=1; i < setup.size(); ++i){ Link* pinLink = link(setup[i].toString()); if(pinLink){ pinDragIK->setPin(pinLink, PinDragIK::TRANSLATION | PinDragIK::ROTATION); } } pinDragIK->initialize(); ik = pinDragIK; */ } } } } return ik; } void Body::setVirtualJointForces() { if(customizerInterface && customizerInterface->setVirtualJointForces){ customizerInterface->setVirtualJointForces(customizerHandle); } } void Body::clearExternalForces() { int n = linkTraverse_.numLinks(); for(int i=0; i < n; ++i){ Link* link = linkTraverse_[i]; link->fext.setZero(); link->tauext.setZero(); } } void Body::updateLinkColdetModelPositions() { const int n = linkTraverse_.numLinks(); for(int i=0; i < n; ++i){ Link* link = linkTraverse_[i]; if(link->coldetModel){ link->coldetModel->setPosition(link->segmentAttitude(), link->p); } } } void Body::putInformation(std::ostream &out) { out << "Body: model name = " << modelName_ << " name = " << name_ << "\n\n"; int n = numLinks(); for(int i=0; i < n; ++i){ out << *link(i); } out << std::endl; } /** The function installs the pre-loaded customizer corresponding to the model name. */ bool Body::installCustomizer() { if(!pluginsInDefaultDirectoriesLoaded){ loadBodyCustomizers(bodyInterface()); pluginsInDefaultDirectoriesLoaded = true; } BodyCustomizerInterface* interface = findBodyCustomizer(modelName_); return interface ? installCustomizer(interface) : false; } bool Body::installCustomizer(BodyCustomizerInterface * customizerInterface) { if(this->customizerInterface){ if(customizerHandle){ this->customizerInterface->destroy(customizerHandle); customizerHandle = 0; } this->customizerInterface = 0; } if(customizerInterface){ customizerHandle = customizerInterface->create(bodyHandle, modelName_.c_str()); if(customizerHandle){ this->customizerInterface = customizerInterface; } } return (customizerHandle != 0); } std::ostream& operator<< (std::ostream& out, Body& body) { body.putInformation(out); return out; } static inline Link* extractLink(BodyHandle bodyHandle, int linkIndex) { return static_cast(bodyHandle)->body->link(linkIndex); } static int getLinkIndexFromName(BodyHandle bodyHandle, const char* linkName) { Body* body = static_cast(bodyHandle)->body; Link* link = body->link(linkName); return (link ? link->index : -1); } static const char* getLinkName(BodyHandle bodyHandle, int linkIndex) { return extractLink(bodyHandle, linkIndex)->name().c_str(); } static double* getJointValuePtr(BodyHandle bodyHandle, int linkIndex) { return &(extractLink(bodyHandle,linkIndex)->q); } static double* getJointVelocityPtr(BodyHandle bodyHandle, int linkIndex) { return &(extractLink(bodyHandle, linkIndex)->dq); } static double* getJointTorqueForcePtr(BodyHandle bodyHandle, int linkIndex) { return &(extractLink(bodyHandle, linkIndex)->u); } BodyInterface* Body::bodyInterface() { static BodyInterface interface = { BODY_INTERFACE_VERSION, getLinkIndexFromName, getLinkName, getJointValuePtr, getJointVelocityPtr, getJointTorqueForcePtr, }; return &interface; } CustomizedJointPath::CustomizedJointPath(Body* body, Link* baseLink, Link* targetLink) : JointPath(baseLink, targetLink), body(body) { onJointPathUpdated(); } CustomizedJointPath::~CustomizedJointPath() { } void CustomizedJointPath::onJointPathUpdated() { ikTypeId = body->customizerInterface->initializeAnalyticIk( body->customizerHandle, baseLink()->index, endLink()->index); if(ikTypeId){ isCustomizedIkPathReversed = false; } else { // try reversed path ikTypeId = body->customizerInterface->initializeAnalyticIk( body->customizerHandle, endLink()->index, baseLink()->index); if(ikTypeId){ isCustomizedIkPathReversed = true; } } } bool CustomizedJointPath::calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R) { bool solved; if(ikTypeId == 0 || isBestEffortIKMode){ solved = JointPath::calcInverseKinematics(end_p, end_R); } else { #if 0 std::vector qorg(numJoints()); for(int i=0; i < numJoints(); ++i){ qorg[i] = joint(i)->q; } #endif const Link* targetLink = endLink(); const Link* baseLink_ = baseLink(); Vector3 p_relative; Matrix3 R_relative; if(!isCustomizedIkPathReversed){ p_relative.noalias() = baseLink_->R.transpose() * (end_p - baseLink_->p); R_relative.noalias() = baseLink_->R.transpose() * end_R; } else { p_relative.noalias() = end_R.transpose() * (baseLink_->p - end_p); R_relative.noalias() = end_R.transpose() * baseLink_->R; } solved = body->customizerInterface-> calcAnalyticIk(body->customizerHandle, ikTypeId, p_relative, R_relative); if(solved){ calcForwardKinematics(); #if 0 double errsqr = (end_p - targetLink->p).squaredNorm() + omegaFromRot(targetLink->R.transpose() * end_R).squaredNorm(); if(errsqr < maxIKerrorSqr){ solved = true; } else { solved = false; for(int i=0; i < numJoints(); ++i){ joint(i)->q = qorg[i]; } calcForwardKinematics(); } #endif } } return solved; } bool CustomizedJointPath::hasAnalyticalIK() const { return (ikTypeId != 0); } choreonoid-1.1.0+dfsg/src/Body/Body.h000066400000000000000000000201411207742442300173160ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_BODY_H_INCLUDED #define CNOID_BODY_BODY_H_INCLUDED #include "LinkTraverse.h" #include "LinkGroup.h" #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class Sensor; class Body; typedef boost::intrusive_ptr BodyPtr; class JointPath; typedef boost::shared_ptr JointPathPtr; class InverseKinematics; typedef boost::shared_ptr InverseKinematicsPtr; class LinkGroup; class YamlMapping; typedef boost::intrusive_ptr YamlMappingPtr; struct BodyHandleEntity { Body* body; }; struct BodyInterface; struct BodyCustomizerInterface; typedef void* BodyHandle; typedef void* BodyCustomizerHandle; class CNOID_EXPORT Body : public Referenced { public: static void addCustomizerDirectory(const std::string& path); static BodyInterface* bodyInterface(); Body(); virtual ~Body(); virtual BodyPtr duplicate() const; inline const std::string& name() { return name_; } inline void setName(const std::string& name) { name_ = name; } inline const std::string& modelName() { return modelName_; } inline void setModelName(const std::string& name) { modelName_ = name; } void setRootLink(Link* link); /** This function must be called when the structure of the link tree is changed. */ void updateLinkTree(); /** The number of the links that work as a joint. Note that the acutal value is the maximum joint ID plus one. Thus there may be a case where the value does not correspond to the actual number of the joint-links. In other words, the value represents the size of the link sequence obtained by joint() function. */ inline int numJoints() const { return jointIdToLinkArray.size(); } /** This function returns a link that has a given joint ID. If there is no link that has a given joint ID, the function returns a dummy link object whose ID is minus one. The maximum id can be obtained by numJoints(). */ inline Link* joint(int id) const { return jointIdToLinkArray[id]; } /** The vector corresponding to the sequence of joint(). */ inline const std::vector& joints() const { return jointIdToLinkArray; } /** The number of all the links the body has. The value corresponds to the size of the sequence obtained by link() function. */ inline int numLinks() const { return linkTraverse_.numLinks(); } /** This function returns the link of a given index in the whole link sequence. The order of the sequence corresponds to a link-tree traverse from the root link. The size of the sequence can be obtained by numLinks(). */ inline Link* link(int index) const { return linkTraverse_.link(index); } inline const LinkTraverse& links() const { return linkTraverse_; } /** LinkTraverse object that traverses all the links from the root link */ inline const LinkTraverse& linkTraverse() const { return linkTraverse_; } /** This function returns a link that has a given name. */ Link* link(const std::string& name) const; /** The root link of the body */ inline Link* rootLink() const { return rootLink_; } // sensor access methods Sensor* createSensor(Link* link, int sensorType, int id, const std::string& name); void addSensor(Sensor* sensor, int sensorType, int id ); inline Sensor* sensor(int sensorType, int sensorId) const { return allSensors[sensorType][sensorId]; } inline int numSensors(int sensorType) const { return allSensors[sensorType].size(); } inline int numSensorTypes() const { return allSensors.size(); } void clearSensorValues(); template inline TSensor* sensor(int id) const { return static_cast(allSensors[TSensor::TYPE][id]); } template inline TSensor* sensor(const std::string& name) const { TSensor* sensor = 0; NameToSensorMap::const_iterator p = nameToSensorMap.find(name); if(p != nameToSensorMap.end()){ sensor = dynamic_cast(p->second); } return sensor; } /** This function returns true when the whole body is a static, fixed object like a floor. */ inline bool isStaticModel() { return isStaticModel_; } double calcTotalMass(); inline double totalMass() const { return totalMass_; } Vector3 calcCM(); const Vector3& lastCM() { return lastCM_; } void calcTotalMomentum(Vector3& out_P, Vector3& out_L); void setDefaultRootPosition(const Vector3& p, const Matrix3& R); void getDefaultRootPosition(Vector3& out_p, Matrix3& out_R); void initializeConfiguration(); void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false); void clearExternalForces(); JointPathPtr getJointPath(Link* baseLink, Link* targetLink); void setVirtualJointForces(); virtual InverseKinematicsPtr getDefaultIK(Link* targetLink); /** This function must be called before the collision detection. It updates the positions and orientations of the models for detecting collisions between links. */ void updateLinkColdetModelPositions(); void putInformation(std::ostream &out); bool installCustomizer(); bool installCustomizer(BodyCustomizerInterface* customizerInterface); struct LinkConnection { Link* link[2]; Vector3 point[2]; int numConstraintAxes; Vector3 constraintAxes[3]; }; typedef std::vector LinkConnectionArray; LinkConnectionArray linkConnections; YamlMapping* info() { return info_.get(); } void resetInfo(YamlMappingPtr info); LinkGroup* linkGroup() { return linkGroup_.get(); } protected: Body(const Body& org); virtual void doResetInfo(const YamlMapping& info); private: bool isStaticModel_; Link* rootLink_; std::string name_; std::string modelName_; typedef std::vector LinkArray; LinkArray jointIdToLinkArray; LinkTraverse linkTraverse_; typedef std::map NameToLinkMap; NameToLinkMap nameToLinkMap; // sensor = sensors[type][sensorId] typedef std::vector SensorArray; std::vector allSensors; typedef std::map NameToSensorMap; NameToSensorMap nameToSensorMap; double totalMass_; Vector3 lastCM_; Vector3 defaultRootPosition; Matrix3 defaultRootAttitude; LinkGroupPtr linkGroup_; YamlMappingPtr info_; // Members for customizer BodyCustomizerHandle customizerHandle; BodyCustomizerInterface* customizerInterface; BodyHandleEntity bodyHandleEntity; BodyHandle bodyHandle; void initialize(); Link* createEmptyJoint(int jointId); void setVirtualJointForcesSub(); friend class CustomizedJointPath; }; }; CNOID_EXPORT std::ostream &operator<< (std::ostream& out, cnoid::Body& body); #endif choreonoid-1.1.0+dfsg/src/Body/BodyCustomizerInterface.cpp000066400000000000000000000164631207742442300235730ustar00rootroot00000000000000/** \file \brief The implementation for the BodyCustomizer class \author Shin'ichiro Nakaoka */ #include "BodyCustomizerInterface.h" #include "Body.h" #include #include #include #include #include #include #if (BOOST_VERSION <= 103301) #include #include #else #include #endif using namespace cnoid; using namespace std; using namespace boost; namespace { #ifdef _WIN32 # include const char* DLLSFX = ".dll"; const char* PATH_DELIMITER = ";"; const char* CNOID_BODY_SHARE_DIR=""; typedef HINSTANCE DllHandle; inline DllHandle loadDll(const char* filename) { return LoadLibrary(filename); } inline void* resolveDllSymbol(DllHandle handle, const char* symbol) { return GetProcAddress(handle, symbol); } inline void unloadDll(DllHandle handle) { FreeLibrary(handle); } #else # include #ifdef __darwin__ const char* DLLSFX = ".dylib"; #else const char* DLLSFX = ".so"; #endif const char* PATH_DELIMITER = ":"; typedef void* DllHandle; inline DllHandle loadDll(const char* filename) { return dlopen(filename, RTLD_LAZY); } inline void* resolveDllSymbol(DllHandle handle, const char* symbol) { return dlsym(handle, symbol); } inline void unloadDll(DllHandle handle) { dlclose(handle); } #endif typedef std::map NameToInterfaceMap; NameToInterfaceMap customizerRepository; bool pluginLoadingFunctionsCalled = false; inline string toNativePathString(const filesystem::path& path) { #if (BOOST_VERSION <= 103301) return path.native_file_string(); #else return path.file_string(); #endif } set customizerDirectories; } static bool checkInterface(BodyCustomizerInterface* customizerInterface) { bool qualified = true && (customizerInterface->version == BODY_CUSTOMIZER_INTERFACE_VERSION) && customizerInterface->getTargetModelNames && customizerInterface->create && customizerInterface->destroy; //&& customizerInterface->initializeAnalyticIk //&& customizerInterface->calcAnalyticIk //&& customizerInterface->setVirtualJointForces; return qualified; } static bool loadCustomizerDll(BodyInterface* bodyInterface, const std::string filename) { BodyCustomizerInterface* customizerInterface = 0; DllHandle dll = loadDll(filename.c_str()); if(dll){ GetBodyCustomizerInterfaceFunc getCustomizerInterface = (GetBodyCustomizerInterfaceFunc)resolveDllSymbol(dll, "getHrpBodyCustomizerInterface"); if(!getCustomizerInterface){ unloadDll(dll); } else { customizerInterface = getCustomizerInterface(bodyInterface); if(customizerInterface){ if(!checkInterface(customizerInterface)){ cerr << "Body customizer \"" << filename << "\" is incomatible and cannot be loaded."; } else { cerr << "Loading body customizer \"" << filename << "\" for "; const char** names = customizerInterface->getTargetModelNames(); for(int i=0; names[i]; ++i){ if(i > 0){ cerr << ", "; } string name(names[i]); if(!name.empty()){ customizerRepository[name] = customizerInterface; } cerr << names[i]; } cerr << endl; } } } } return (customizerInterface != 0); } /** DLLs of body customizer in the path are loaded and they are registered to the customizer repository. The loaded customizers can be obtained by using findBodyCustomizer() function. \param pathString the path to a DLL file or a directory that contains DLLs */ int cnoid::loadBodyCustomizers(const std::string pathString, BodyInterface* bodyInterface) { pluginLoadingFunctionsCalled = true; int numLoaded = 0; filesystem::path pluginPath(pathString, filesystem::native); if(filesystem::exists(pluginPath)){ if(!filesystem::is_directory(pluginPath)){ if(loadCustomizerDll(bodyInterface, toNativePathString(pluginPath))){ numLoaded++; } } else { static const string pluginNamePattern(string("Customizer") + DLLSFX); filesystem::directory_iterator end; for(filesystem::directory_iterator it(pluginPath); it != end; ++it){ const filesystem::path& filepath = *it; if(!filesystem::is_directory(filepath)){ string filename(filepath.leaf()); size_t pos = filename.rfind(pluginNamePattern); if(pos == (filename.size() - pluginNamePattern.size())){ if(loadCustomizerDll(bodyInterface, toNativePathString(filepath))){ numLoaded++; } } } } } } return numLoaded; } int cnoid::loadBodyCustomizers(const std::string pathString) { return loadBodyCustomizers(pathString, Body::bodyInterface()); } /** The function loads the customizers in the directories specified by the environmental variable CNOID_CUSTOMIZER_PATH */ int cnoid::loadBodyCustomizers(BodyInterface* bodyInterface) { int numLoaded = 0; if(!pluginLoadingFunctionsCalled){ pluginLoadingFunctionsCalled = true; char* pathListEnv = getenv("CNOID_CUSTOMIZER_PATH"); if(pathListEnv){ char_separator sep(PATH_DELIMITER); string pathList(pathListEnv); tokenizer< char_separator > paths(pathList, sep); tokenizer< char_separator >::iterator p; for(p = paths.begin(); p != paths.end(); ++p){ numLoaded = loadBodyCustomizers(*p, bodyInterface); } } #ifndef _WIN32 Dl_info info; if(dladdr((void*)&findBodyCustomizer, &info)){ filesystem::path customizerPath = filesystem::path(info.dli_fname).branch_path().branch_path() / CNOID_PLUGIN_SUBDIR / "customizer"; customizerDirectories.insert(customizerPath.string()); } #else string customizerPath(CNOID_BODY_SHARE_DIR); customizerPath.append("/customizer"); customizerDirectories.insert(string(CNOID_BODY_SHARE_DIR) + "/customzier"); #endif for(std::set::iterator p = customizerDirectories.begin(); p != customizerDirectories.end(); ++p){ numLoaded += loadBodyCustomizers(*p, bodyInterface); } } return numLoaded; } int cnoid::loadBodyCustomizers() { return loadBodyCustomizers(Body::bodyInterface()); } BodyCustomizerInterface* cnoid::findBodyCustomizer(std::string modelName) { BodyCustomizerInterface* customizerInterface = 0; NameToInterfaceMap::iterator p = customizerRepository.find(modelName); if(p != customizerRepository.end()){ customizerInterface = p->second; } return customizerInterface; } void Body::addCustomizerDirectory(const std::string& path) { customizerDirectories.insert(path); } choreonoid-1.1.0+dfsg/src/Body/BodyCustomizerInterface.h000066400000000000000000000054461207742442300232370ustar00rootroot00000000000000/** \file \brief The definitions of the body customizer interface for increasing binary compatibility \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_CUSTOMIZER_INTERFACE_H_INCLUDED #define CNOID_BODY_CUSTOMIZER_INTERFACE_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { typedef void* BodyHandle; typedef void* BodyCustomizerHandle; typedef int (*BodyGetLinkIndexFromNameFunc) (BodyHandle bodyHandle, const char* linkName); typedef const char* (*BodyGetLinkNameFunc) (BodyHandle bodyHandle, int linkIndex); typedef double* (*BodyGetLinkDoubleValuePtrFunc)(BodyHandle bodyHandle, int linkIndex); static const int BODY_INTERFACE_VERSION = 1; struct BodyInterface { int version; BodyGetLinkIndexFromNameFunc getLinkIndexFromName; BodyGetLinkNameFunc getLinkName; BodyGetLinkDoubleValuePtrFunc getJointValuePtr; BodyGetLinkDoubleValuePtrFunc getJointVelocityPtr; BodyGetLinkDoubleValuePtrFunc getJointForcePtr; }; typedef const char** (*BodyCustomizerGetTargetModelNamesFunc)(); typedef BodyCustomizerHandle (*BodyCustomizerCreateFunc)(BodyHandle bodyHandle, const char* modelName); typedef void (*BodyCustomizerDestroyFunc) (BodyCustomizerHandle customizerHandle); typedef int (*BodyCustomizerInitializeAnalyticIkFunc) (BodyCustomizerHandle customizerHandle, int baseLinkIndex, int targetLinkIndex); /* p and R are based on the coordinate of a base link */ typedef bool (*BodyCustomizerCalcAnalyticIkFunc) (BodyCustomizerHandle customizerHandle, int ikPathId, const Vector3& p, const Matrix3& R); typedef void (*BodyCustomizerSetVirtualJointForcesFunc)(BodyCustomizerHandle customizerHandle); static const int BODY_CUSTOMIZER_INTERFACE_VERSION = 1; struct BodyCustomizerInterface { int version; BodyCustomizerGetTargetModelNamesFunc getTargetModelNames; BodyCustomizerCreateFunc create; BodyCustomizerDestroyFunc destroy; BodyCustomizerInitializeAnalyticIkFunc initializeAnalyticIk; BodyCustomizerCalcAnalyticIkFunc calcAnalyticIk; BodyCustomizerSetVirtualJointForcesFunc setVirtualJointForces; }; typedef BodyCustomizerInterface* (*GetBodyCustomizerInterfaceFunc)(BodyInterface* bodyInterface); CNOID_EXPORT int loadBodyCustomizers(const std::string pathString, BodyInterface* bodyInterface); CNOID_EXPORT int loadBodyCustomizers(const std::string pathString); CNOID_EXPORT int loadBodyCustomizers(BodyInterface* bodyInterface); CNOID_EXPORT int loadBodyCustomizers(); CNOID_EXPORT BodyCustomizerInterface* findBodyCustomizer(std::string modelName); } #endif choreonoid-1.1.0+dfsg/src/Body/BodyLoader.cpp000066400000000000000000000444631207742442300210150ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "BodyLoader.h" #include "Link.h" #include "Sensor.h" #include "ModelNodeSet.h" #include #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class BodyLoaderImpl { public: BodyLoaderImpl(); ~BodyLoaderImpl(); void setDivisionNumber(int n); BodyPtr loadModelFile( const std::string& filename, bool doTriangulation, bool doNormalGeneration, bool createColdetModel); ModelNodeSetPtr modelNodeSet; TriangleMeshShaper triangleMeshShaper; BodyPtr body; bool createColdetModel; string errorMessage; YamlMappingPtr info; boost::signal sigMessage; typedef map SensorTypeMap; static SensorTypeMap sensorTypeMap; struct MeshInfo { EIGEN_MAKE_ALIGNED_OPERATOR_NEW MeshInfo(VrmlIndexedFaceSet* triangleMesh, const Affine3& TX) : triangleMesh(triangleMesh), TX(TX) { } VrmlIndexedFaceSet* triangleMesh; Affine3 TX; }; vector > meshes; int numTotalVertices; int numTotalTriangles; void createBodyFromModelNodeSet(); Link* createLink(JointNodeSetPtr jointNodeSet, const Matrix3& parentRs); void setJointProperties(Link* link, VrmlProtoInstancePtr jointNode, const Matrix3& parentRs); void setSegmentProperties(Link* link, vector segmentNodes, const JointNodeSet::Affine3Array& transforms); void createSensors(Link* link, std::vector& sensorNodes); void setColdetModel(Link* link, vector segmentNodes, const JointNodeSet::Affine3Array& transforms); bool collectMeshes(VrmlShape* shapeNode, const Affine3& T); void extractShapeNodes(VrmlNode* node, const function& callback); bool extractShapeNodeTraverse( VrmlNode* node, const Affine3& T, const function& callback); }; } BodyLoaderImpl::SensorTypeMap BodyLoaderImpl::sensorTypeMap; namespace { double getLimitValue(VrmlVariantField& field, double defaultValue) { MFFloat& values = get(field); if(values.empty()){ return defaultValue; } return values[0]; } void copyVrmlField(VrmlVariantField& field, string& out_s) { switch(field.which()){ case SFSTRING: out_s = get(field); break; case MFSTRING: { MFString& strings = get(field); out_s = ""; for(size_t i=0; i < strings.size(); i++){ out_s += strings[i] + "\n"; } } break; default: break; } } void copyVrmlField(VrmlVariantField& field, int& out_value) { out_value = get(field); } void copyVrmlField(VrmlVariantField& field, double& out_value) { out_value = get(field); } void copyVrmlField(VrmlVariantField& field, Vector3& out_v) { out_v = get(field); } void copyVrmlField(VrmlVariantField& field, Matrix3& out_R) { if(field.which() == SFROTATION){ out_R = get(field).toRotationMatrix(); } else if(field.which() == MFFLOAT){ MFFloat& mf = get(field); if(mf.size() >= 9){ out_R << mf[0], mf[1], mf[2], mf[3], mf[4], mf[5], mf[6], mf[7], mf[8]; } } } } BodyLoader::BodyLoader() { impl = new BodyLoaderImpl(); } BodyLoaderImpl::BodyLoaderImpl() { if(sensorTypeMap.empty()){ sensorTypeMap["ForceSensor"] = Sensor::FORCE; sensorTypeMap["Gyro"] = Sensor::RATE_GYRO; sensorTypeMap["AccelerationSensor"] = Sensor::ACCELERATION; sensorTypeMap["PressureSensor"] = Sensor::PRESSURE; sensorTypeMap["PhotoInterrupter"] = Sensor::PHOTO_INTERRUPTER; sensorTypeMap["VisionSensor"] = Sensor::VISION; sensorTypeMap["TorqueSensor"] = Sensor::TORQUE; } triangleMeshShaper.sigMessage.connect(sigMessage); } BodyLoader::~BodyLoader() { delete impl; } BodyLoaderImpl::~BodyLoaderImpl() { } ModelNodeSetPtr BodyLoader::modelNodeSet() { return impl->modelNodeSet; } const std::string& BodyLoader::errorMessage() { return impl->errorMessage; } SignalProxy< boost::signal > BodyLoader::sigMessage() { return impl->sigMessage; } /** The function for setting the division number of primitive geometries such as cone, cylinder and sphere. */ void BodyLoader::setDivisionNumber(int n) { impl->setDivisionNumber(n); } void BodyLoaderImpl::setDivisionNumber(int n) { triangleMeshShaper.setDivisionNumber(std::max(4, n)); } BodyPtr BodyLoader::loadModelFile (const std::string& filename, bool doTriangulation, bool doNormalGeneration, bool createColdetModel) { return impl->loadModelFile(filename, doTriangulation, doNormalGeneration, createColdetModel); } BodyPtr BodyLoaderImpl::loadModelFile (const std::string& filename, bool doTriangulation, bool doNormalGeneration, bool createColdetModel) { body = 0; info = 0; errorMessage.clear(); filesystem::path orgpath(filename); string ext = filesystem::extension(orgpath); string vrmlFile; try { if(ext == ".wrl"){ vrmlFile = filename; } else if(ext == ".yaml"){ YamlReader parser; if(parser.load(filename)){ info = parser.document()->toMapping(); filesystem::path vrmlFilePath(info->get("modelFile")); if(vrmlFilePath.has_root_path()){ vrmlFile = vrmlFilePath.file_string(); } else { vrmlFile = (orgpath.branch_path() / vrmlFilePath).file_string(); } } } if(!vrmlFile.empty()){ if(info){ int divisionNumber; if(info->read("divisionNumberOfPrimitiveGeometries", divisionNumber)){ setDivisionNumber(divisionNumber); } } modelNodeSet.reset(new ModelNodeSet()); if(modelNodeSet->loadModelFile(vrmlFile)){ if(doTriangulation || createColdetModel){ triangleMeshShaper.setNormalGenerationMode(doNormalGeneration); triangleMeshShaper.apply(modelNodeSet->humanoidNode()); } this->createColdetModel = createColdetModel; createBodyFromModelNodeSet(); } } } catch(const YamlNode::Exception& ex){ errorMessage = ex.message(); } catch(ModelNodeSet::Exception& ex){ errorMessage = ex.what(); } return body; } void BodyLoaderImpl::createBodyFromModelNodeSet() { JointNodeSetPtr rootJointNodeSet = modelNodeSet->rootJointNodeSet(); if(rootJointNodeSet){ if(info){ if(LeggedBody::checkBodyInfoAsLeggedBody(info)){ body = new LeggedBody(); } } if(!body){ body = new Body(); } body->setModelName(modelNodeSet->humanoidNode()->defName); Matrix3 Rs = Matrix3::Identity(); Link* rootLink = createLink(rootJointNodeSet, Rs); body->setRootLink(rootLink); TProtoFieldMap& f = rootJointNodeSet->jointNode->fields; Vector3 defaultRootPos; copyVrmlField(f["translation"], defaultRootPos); Matrix3 defaultRootR; copyVrmlField(f["rotation"], defaultRootR); body->setDefaultRootPosition(defaultRootPos, defaultRootR); body->installCustomizer(); body->initializeConfiguration(); if(info){ body->resetInfo(info); } } } Link* BodyLoaderImpl::createLink(JointNodeSetPtr jointNodeSet, const Matrix3& parentRs) { Link* link = new Link(); setJointProperties(link, jointNodeSet->jointNode, parentRs); vector segmentNodes = jointNodeSet->segmentNodes; const JointNodeSet::Affine3Array& transforms = jointNodeSet->transforms; setSegmentProperties(link, segmentNodes, transforms); if(createColdetModel && !segmentNodes.empty()){ setColdetModel(link, segmentNodes, transforms); } // The following code adds child links from the back of the child array // in order to keep the original order of the children. // ( addChild() function of the Link class prepends a child to the child list ) int numChildren = jointNodeSet->childJointNodeSets.size(); for(int i = numChildren - 1; i >= 0; --i){ JointNodeSetPtr childJointNodeSet = jointNodeSet->childJointNodeSets[i]; Link* childLink = createLink(childJointNodeSet, link->Rs); link->addChild(childLink); } createSensors(link, jointNodeSet->sensorNodes); return link; } void BodyLoaderImpl::setJointProperties(Link* link, VrmlProtoInstancePtr jointNode, const Matrix3& parentRs) { link->setName(jointNode->defName); TProtoFieldMap& jf = jointNode->fields; copyVrmlField(jf["jointId"], link->jointId); Vector3 b; copyVrmlField(jf["translation"], b); link->b.noalias() = parentRs * b; Matrix3 R; copyVrmlField(jf["rotation"], R); link->Rs.noalias() = parentRs * R; Vector3 jointAxis = Vector3::Zero(); VrmlVariantField& jointAxisField = jf["jointAxis"]; switch(jointAxisField.which()){ case SFSTRING: { SFString& axisLabel = get(jointAxisField); if(axisLabel == "X"){ jointAxis = Vector3::UnitX(); } else if(axisLabel == "Y"){ jointAxis = Vector3::UnitY(); } else if(axisLabel == "Z"){ jointAxis = Vector3::UnitZ(); } } break; case SFVEC3F: copyVrmlField(jointAxisField, jointAxis); break; default: break; } string jointType; copyVrmlField(jf["jointType"], jointType); if(jointType == "fixed" ){ link->jointType = Link::FIXED_JOINT; } else if(jointType == "free" ){ link->jointType = Link::FREE_JOINT; } else if(jointType == "rotate" ){ link->jointType = Link::ROTATIONAL_JOINT; } else if(jointType == "slide" ){ link->jointType = Link::SLIDE_JOINT; } else { link->jointType = Link::FREE_JOINT; } link->a.setZero(); link->d.setZero(); if(link->jointType == Link::ROTATIONAL_JOINT){ link->a.noalias() = link->Rs * jointAxis; } else if(link->jointType == Link::SLIDE_JOINT){ link->d.noalias() = link->Rs * jointAxis; } copyVrmlField(jf["jointValue"], link->defaultJointValue); copyVrmlField(jf["rotorInertia"], link->Ir); copyVrmlField(jf["gearRatio"], link->gearRatio); copyVrmlField(jf["torqueConst"], link->torqueConst); copyVrmlField(jf["encoderPulse"], link->encoderPulse); copyVrmlField(jf["rotorResistor"], link->rotorResistor); VrmlVariantField* field = jointNode->findField("equivalentInertia"); if(field){ link->Jm2 = get(*field); } else { link->Jm2 = link->gearRatio * link->gearRatio * link->Ir; } double max = numeric_limits::max(); link->ulimit = getLimitValue(jf["ulimit"], +max); link->llimit = getLimitValue(jf["llimit"], -max); link->uvlimit = getLimitValue(jf["uvlimit"], +max); link->lvlimit = getLimitValue(jf["lvlimit"], -max); } void BodyLoaderImpl::setSegmentProperties (Link* link, vector segmentNodes, const JointNodeSet::Affine3Array& transforms) { int numSegment = segmentNodes.size(); link->m = 0.0; // Mass = Σmass // // C = (Σmass * T * c) / Mass // // I = Σ(R * I * Rt + G) // // R = Tã®å›žè»¢è¡Œåˆ— // // G = y*y+z*z, -x*y, -x*z, -y*x, z*z+x*x, -y*z, -z*x, -z*y, x*x+y*y // // (x, y, z ) = T * c - C // std::vector centerOfMassArray; std::vector massArray; Vector3 linkc = Vector3::Zero(); Matrix3 linkI = Matrix3::Zero(); for(int i = 0 ; i < numSegment ; ++i){ const Affine3& T = transforms[i]; TProtoFieldMap& sf = segmentNodes[i]->fields; Vector3 c; copyVrmlField(sf["centerOfMass"], c); double m; copyVrmlField(sf["mass"], m); Matrix3 I; copyVrmlField(sf["momentsOfInertia"], I); //Matrix3 R(T.linear()); const Vector3 c1 = T.linear() * c + T.translation(); centerOfMassArray.push_back(c1); massArray.push_back(m); linkc = c1 * m + linkc * link->m; link->m += m; linkc /= link->m; linkI.noalias() += T.linear() * I * T.linear().transpose(); } for(int i = 0 ; i < numSegment ; ++i){ const Vector3& c = centerOfMassArray[i]; double x = c(0) - linkc(0); double y = c(1) - linkc(1); double z = c(2) - linkc(2); double m = massArray[i]; linkI(0,0) += m * (y*y + z*z); linkI(0,1) += -m * x * y; linkI(0,2) += -m * x * z; linkI(1,0) += -m * y * x; linkI(1,1) += m * (z*z + x*x); linkI(1,2) += -m * y * z; linkI(2,0) += -m * z * x; linkI(2,1) += -m * z * y; linkI(2,2) += m * (x*x + y*y); } link->c.noalias() = link->Rs * linkc; link->I.noalias() = link->Rs * linkI * link->Rs.transpose(); } void BodyLoaderImpl::createSensors(Link* link, std::vector& sensorNodes) { int numSensors = sensorNodes.size(); for(int i=0; i < numSensors; ++i){ VrmlProtoInstancePtr sensorNode = sensorNodes[i]; TProtoFieldMap& f = sensorNode->fields; const string& name = sensorNode->defName; int id; copyVrmlField(f["sensorId"], id); if(id < 0){ throw ModelNodeSet::Exception (string("sensor ID is not given to sensor ") + name + "of model " + body->modelName()); } else { int sensorType = Sensor::COMMON; SensorTypeMap::iterator p = sensorTypeMap.find(sensorNode->proto->protoName); if(p != sensorTypeMap.end()){ sensorType = p->second; } else { throw ModelNodeSet::Exception("Unknown type sensor node"); } Sensor* sensor = body->createSensor(link, sensorType, id, name); if(sensor){ Vector3 p; copyVrmlField(f["translation"], p); sensor->localPos.noalias() = link->Rs * p; Matrix3 R; copyVrmlField(f["rotation"], R); sensor->localR.noalias() = link->Rs * R; } } } } void BodyLoaderImpl::setColdetModel (Link* link, vector segmentNodes, const JointNodeSet::Affine3Array& transforms) { meshes.clear(); numTotalVertices = 0; numTotalTriangles = 0; int numSegment = segmentNodes.size(); for(int i = 0 ; i < numSegment ; ++i){ extractShapeNodeTraverse(segmentNodes[i].get(), transforms[i], bind(&BodyLoaderImpl::collectMeshes, this, _1, _2)); } ColdetModelPtr coldetModel(new ColdetModel()); coldetModel->setNumVertices(numTotalVertices); coldetModel->setNumTriangles(numTotalTriangles); int vertexIndex = 0; int triangleIndex = 0; for(size_t i=0; i < meshes.size(); ++i){ int vertexTop = vertexIndex; Affine3 T = Affine3(link->Rs) * meshes[i].TX; const MFVec3f& vertices = meshes[i].triangleMesh->coord->point; size_t numVertices = vertices.size(); for(size_t j=0; j < numVertices; ++j){ Vector3 v = T * vertices[j]; coldetModel->setVertex(vertexIndex++, (float)v[0], (float)v[1], (float)v[2]); } const MFInt32& indices = meshes[i].triangleMesh->coordIndex; const size_t numTriangles = indices.size() / 4; for(size_t j=0; j < numTriangles; ++j){ coldetModel->setTriangle( triangleIndex++, vertexTop + indices[j*4], vertexTop + indices[j*4+1], vertexTop + indices[j*4+2]); } } coldetModel->setName(link->name()); coldetModel->build(); link->coldetModel = coldetModel; } bool BodyLoaderImpl::collectMeshes(VrmlShape* shapeNode, const Affine3& T) { VrmlIndexedFaceSet* triangleMesh = dynamic_node_cast(shapeNode->geometry).get(); if(triangleMesh){ meshes.push_back(MeshInfo(triangleMesh, T)); numTotalVertices += triangleMesh->coord->point.size(); numTotalTriangles += triangleMesh->coordIndex.size() / 4; } return true; } /** @todo move this code into the ModelNodeSet class ? */ bool BodyLoaderImpl::extractShapeNodeTraverse (VrmlNode* node, const Affine3& T, const function& callback) { bool doContinue = true; if(node->isCategoryOf(PROTO_INSTANCE_NODE)){ VrmlProtoInstance* protoInstance = static_cast(node); if(protoInstance->actualNode){ doContinue = extractShapeNodeTraverse(protoInstance->actualNode.get(), T, callback); } } else if(node->isCategoryOf(GROUPING_NODE)) { VrmlGroup* groupNode = static_cast(node); Affine3 T2; VrmlTransform* transformNode = dynamic_cast(groupNode); if(transformNode){ T2 = T * transformNode->toAffine3d(); } VrmlSwitch* switchNode = dynamic_cast(node); if(switchNode){ int whichChoice = switchNode->whichChoice; if(whichChoice >= 0 && whichChoice < switchNode->countChildren()){ extractShapeNodeTraverse(switchNode->getChild(whichChoice), (transformNode ? T2 : T), callback); } } else { for(int i=0; i < groupNode->countChildren(); ++i){ if(!extractShapeNodeTraverse(groupNode->getChild(i), (transformNode ? T2 : T), callback)){ doContinue = false; break; } } } } else if(node->isCategoryOf(SHAPE_NODE)) { doContinue = callback(static_cast(node), T); } return doContinue; } choreonoid-1.1.0+dfsg/src/Body/BodyLoader.h000066400000000000000000000016121207742442300204470ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_BODY_LOADER_H_INCLUDED #define CNOID_BODY_BODY_LOADER_H_INCLUDED #include "Body.h" #include #include "exportdecl.h" namespace cnoid { class ModelNodeSet; typedef boost::shared_ptr ModelNodeSetPtr; class BodyLoaderImpl; class CNOID_EXPORT BodyLoader { public: BodyLoader(); ~BodyLoader(); void setDivisionNumber(int n); BodyPtr loadModelFile( const std::string& filename, bool doTriangulation = true, bool doNormalGeneration = true, bool createColdetModel = true); const std::string& errorMessage(); ModelNodeSetPtr modelNodeSet(); SignalProxy< boost::signal > sigMessage(); private: BodyLoaderImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/Body/BodyMotion.cpp000066400000000000000000000157421207742442300210520ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "BodyMotion.h" #include "Body.h" #include "Link.h" #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { //bool TRACE_FUNCTIONS = false; } BodyMotion::BodyMotion() : MultiSeqBase("BodyMotion"), jointPosSeq_(new MultiValueSeq()), linkPosSeq_(new MultiAffine3Seq()) { jointPosSeq_->setPurpose("JointPosition"); linkPosSeq_->setPurpose("LinkPosition"); } BodyMotion::BodyMotion(const BodyMotion& org) : MultiSeqBase(org), jointPosSeq_(new MultiValueSeq(*org.jointPosSeq_)), linkPosSeq_(new MultiAffine3Seq(*org.linkPosSeq_)) { if(org.relativeZmpSeq_){ relativeZmpSeq_.reset(new Vector3Seq(*org.relativeZmpSeq_)); } } const Vector3SeqPtr& BodyMotion::relativeZmpSeq() { if(!relativeZmpSeq_){ relativeZmpSeq_.reset(new Vector3Seq()); relativeZmpSeq_->setPurpose("RelativeZmp"); relativeZmpSeq_->setNumFrames(numFrames()); relativeZmpSeq_->setFrameRate(frameRate()); } return relativeZmpSeq_; } /* This function sets the number of joints */ void BodyMotion::setNumParts(int numParts, bool clearNewElements) { jointPosSeq_->setNumParts(numParts, clearNewElements); } /** This function returns the number of joints */ int BodyMotion::getNumParts() const { return jointPosSeq_->numParts(); } double BodyMotion::getFrameRate() const { return frameRate(); } void BodyMotion::setFrameRate(double frameRate) { jointPosSeq_->setFrameRate(frameRate); linkPosSeq_->setFrameRate(frameRate); if(relativeZmpSeq_){ relativeZmpSeq_->setFrameRate(frameRate); } } int BodyMotion::getNumFrames() const { return numFrames(); } void BodyMotion::setNumFrames(int n, bool clearNewArea) { jointPosSeq_->setNumFrames(n, clearNewArea); linkPosSeq_->setNumFrames(n, clearNewArea); if(relativeZmpSeq_){ relativeZmpSeq_->setNumFrames(n, clearNewArea); } } void BodyMotion::setDimension(int numFrames, int numJoints, int numLinks, bool clearNewArea) { jointPosSeq_->setDimension(numFrames, numJoints, clearNewArea); linkPosSeq_->setDimension(numFrames, numLinks, clearNewArea); if(relativeZmpSeq_){ relativeZmpSeq_->setNumFrames(numFrames, clearNewArea); } } void BodyMotion::setDimension(int numFrames, int numJoints, bool clearNewArea) { jointPosSeq_->setDimension(numFrames, numJoints, clearNewArea); linkPosSeq_->setNumFrames(numFrames, clearNewArea); if(relativeZmpSeq_){ relativeZmpSeq_->setNumFrames(numFrames, clearNewArea); } } bool BodyMotion::loadStandardYamlFormat(const std::string& filename) { bool result = false; YamlReader reader; reader.expectRegularMultiSequence(); try { reader.load(filename); const YamlMapping& archive = *reader.document()->toMapping(); result = read(archive); } catch(const YamlNode::Exception& ex){ setIoErrorMessage(ex.message()); result = false; } return result; } bool BodyMotion::read(const YamlMapping& archive) { setDimension(0, 1, 1); bool result = true; bool loaded = false; bool zmpLoaded = false; try { if(archive["type"].toString() != "BodyMotion"){ result = false; } else { const YamlSequence& components = *archive["components"].toSequence(); for(int i=0; i < components.size(); ++i){ const YamlMapping& component = *components[i].toMapping(); const string& type = component["type"]; const string& purpose = component["purpose"]; if(type == "MultiValueSeq" && purpose == "JointPosition"){ result &= jointPosSeq_->read(component); if(result){ loaded = true; } } else if((type == "MultiAffine3Seq" || type == "MultiSe3Seq") && purpose == "LinkPosition"){ result &= linkPosSeq_->read(component); if(result){ loaded = true; } } else if(type == "Vector3Seq" && purpose == "RelativeZmp"){ result = relativeZmpSeq()->read(component); if(result){ zmpLoaded = true; } } if(!result){ break; } } } } catch(const YamlNode::Exception& ex){ setIoErrorMessage(ex.message()); result = false; } if(!result){ setDimension(0, 1, 1); } if(!zmpLoaded){ relativeZmpSeq_.reset(); } return (result && loaded); } bool BodyMotion::write(YamlWriter& writer) { writer.startSequence(); jointPosSeq()->write(writer); linkPosSeq()->write(writer); if(hasRelativeZmpSeq()){ relativeZmpSeq()->write(writer); } writer.endSequence(); return true; } bool BodyMotion::saveAsStandardYamlFormat(const std::string& filename) { YamlWriter writer(filename); writer.setDoubleFormat("%.9g"); writer.putComment("Body motion data set format version 1.0 defined by cnoid-Robotics\n"); writer.startMapping(); writer.putKeyValue("type", "BodyMotion"); writer.putKey("components"); bool result = write(writer); writer.endMapping(); return result; } namespace cnoid { BodyMotion::Frame& operator<<(const BodyMotion::Frame& frame, const Body& body) { BodyMotion& motion = const_cast(frame).motion(); int numJoints = std::min(body.numJoints(), motion.numJoints()); MultiValueSeq::View q = motion.jointPosSeq()->frame(frame.frame()); for(int i=0; i < numJoints; ++i){ q[i] = body.joint(i)->q; } int numLinks = std::min(body.numLinks(), motion.numLinks()); MultiAffine3Seq::View T = motion.linkPosSeq()->frame(frame.frame()); for(int i=0; i < numLinks; ++i){ Link* link = body.link(i); T[i].translation() = link->p; T[i].linear() = link->R; } return const_cast(frame); } BodyMotion::Frame& operator>>(const BodyMotion::Frame& frame, const Body& body) { const BodyMotion& motion = frame.motion(); int numJoints = std::min(body.numJoints(), motion.numJoints()); const MultiValueSeq::View q = motion.jointPosSeq()->frame(frame.frame()); for(int i=0; i < numJoints; ++i){ body.joint(i)->q = q[i]; } int numLinks = std::min(body.numLinks(), motion.numLinks()); const MultiAffine3Seq::View T = motion.linkPosSeq()->frame(frame.frame()); for(int i=0; i < numLinks; ++i){ Link* link = body.link(i); link->p = T[i].translation(); link->R = T[i].linear(); } return const_cast(frame); } } choreonoid-1.1.0+dfsg/src/Body/BodyMotion.h000066400000000000000000000062011207742442300205050ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_BODY_BODY_MOTION_H_INCLUDED #define CNOID_BODY_BODY_MOTION_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT BodyMotion : public MultiSeqBase { public: BodyMotion(); BodyMotion(const BodyMotion& org); virtual void setDimension(int numFrames, int numJoints, bool clearNewArea = false); void setDimension(int numFrames, int numJoints, int numLinks, bool clearNewArea = false); virtual void setNumParts(int numParts, bool clearNewElements = false); virtual int getNumParts() const; inline int numJoints() const { return jointPosSeq_->numParts(); } inline int numLinks() const { return linkPosSeq_->numParts(); } inline double frameRate() const { return jointPosSeq_->frameRate(); } virtual double getFrameRate() const; virtual void setFrameRate(double frameRate); inline int numFrames() const { return std::max(jointPosSeq_->numFrames(), linkPosSeq_->numFrames()); } virtual int getNumFrames() const; virtual void setNumFrames(int n, bool clearNewArea = false); inline MultiValueSeqPtr& jointPosSeq() { return jointPosSeq_; } inline const MultiValueSeqPtr& jointPosSeq() const { return jointPosSeq_; } inline MultiAffine3SeqPtr& linkPosSeq() { return linkPosSeq_; } inline const MultiAffine3SeqPtr& linkPosSeq() const { return linkPosSeq_; } inline bool hasRelativeZmpSeq() { return relativeZmpSeq_; } const Vector3SeqPtr& relativeZmpSeq(); class Frame { const BodyMotion& motion_; int frame_; Frame(const BodyMotion& motion, int frame) : motion_(motion), frame_(frame) { } public: Frame(const Frame& org) : motion_(org.motion_), frame_(org.frame_) { } inline BodyMotion& motion() { return const_cast(motion_); } inline const BodyMotion& motion() const { return motion_; } inline int frame() const { return frame_; } friend class BodyMotion; }; inline Frame frame(int frame) { return Frame(*this, frame); } inline const Frame frame(int frame) const { return Frame(*this, frame); } virtual bool read(const YamlMapping& archive); virtual bool write(YamlWriter& writer); bool loadStandardYamlFormat(const std::string& filename); bool saveAsStandardYamlFormat(const std::string& filename); private: MultiValueSeqPtr jointPosSeq_; MultiAffine3SeqPtr linkPosSeq_; Vector3SeqPtr relativeZmpSeq_; }; typedef boost::shared_ptr BodyMotionPtr; class Body; CNOID_EXPORT BodyMotion::Frame& operator<<(const BodyMotion::Frame& frame, const Body& body); CNOID_EXPORT BodyMotion::Frame& operator>>(const BodyMotion::Frame& frame, const Body& body); } #endif choreonoid-1.1.0+dfsg/src/Body/BodyMotionPoseProvider.cpp000066400000000000000000000115011207742442300234010ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "BodyMotionPoseProvider.h" #include "JointPath.h" using namespace std; using namespace boost; using namespace cnoid; #ifdef _MSC_VER namespace { inline long lround(double x) { return static_cast((x > 0.0) ? floor(x + 0.5) : ceil(x -0.5)); } } #endif BodyMotionPoseProvider::BodyMotionPoseProvider() { } BodyMotionPoseProvider::BodyMotionPoseProvider(BodyPtr body_, BodyMotionPtr motion) { initialize(body_, motion); } void BodyMotionPoseProvider::initialize(BodyPtr body__, BodyMotionPtr motion) { body_ = body__->duplicate(); this->motion = motion; footLinkPositions.reset(new MultiAffine3Seq()); footLinks.clear(); ikPaths.clear(); YamlSequence& footLinkInfos = *body_->info()->findSequence("footLinks"); if(footLinkInfos.isValid()){ for(int i=0; i < footLinkInfos.size(); ++i){ const YamlMapping& footLinkInfo = *footLinkInfos[i].toMapping(); Link* link = body_->link(footLinkInfo["link"].toString()); JointPathPtr ikPath = body_->getJointPath(body_->rootLink(), link); if(ikPath && ikPath->hasAnalyticalIK()){ footLinks.push_back(link); ikPaths.push_back(ikPath); } } } updateMotion(); } bool BodyMotionPoseProvider::updateMotion() { const int numFrames = motion->numFrames(); footLinkPositions->setDimension(numFrames, footLinks.size()); footLinkPositions->setFrameRate(motion->frameRate()); MultiValueSeqPtr qseq = motion->jointPosSeq(); MultiAffine3SeqPtr pseq = motion->linkPosSeq(); if(pseq->numParts() < 1){ return false; } Link* rootLink = body_->rootLink(); minNumJoints = std::min(body_->numJoints(), qseq->numParts()); qTranslated.resize(minNumJoints); for(int frame=0; frame < numFrames; ++frame){ const Affine3& p = pseq->at(frame, 0); rootLink->p = p.translation(); rootLink->R = p.linear(); MultiValueSeq::View q = qseq->frame(frame); for(int i=0; i < minNumJoints; ++i){ body_->joint(i)->q = q[i]; } body_->calcForwardKinematics(); for(size_t i=0; i < footLinks.size(); ++i){ Link* footLink = footLinks[i]; Affine3& p = footLinkPositions->at(frame, i); p.translation() = footLink->p; p.linear() = footLink->R; } } return true; } Body* BodyMotionPoseProvider::body() const { return body_.get(); } double BodyMotionPoseProvider::beginningTime() const { return 0.0; } double BodyMotionPoseProvider::endingTime() const { return (motion->numFrames() - 1) / motion->frameRate(); } bool BodyMotionPoseProvider::seek (double time, int waistLinkIndex, const Vector3& waistTranslation, bool applyWaistTranslation) { int frame = lround(time * motion->frameRate()); if(frame >= motion->numFrames()){ frame = motion->numFrames() - 1; } const MultiValueSeq::View q = motion->jointPosSeq()->frame(frame); for(int i=0; i < minNumJoints; ++i){ qTranslated[i] = q[i]; } if(waistLinkIndex != 0){ return false; } const Affine3& waist = motion->linkPosSeq()->at(frame, 0); p_waist = waist.translation(); R_waist = waist.linear(); if(applyWaistTranslation){ p_waist += waistTranslation; for(size_t i=0; i < footLinks.size(); ++i){ const Affine3& foot = footLinkPositions->at(frame, i); JointPathPtr ikPath = ikPaths[i]; ikPath->calcInverseKinematics(p_waist, R_waist, foot.translation(), foot.linear()); for(int j=0; j < ikPath->numJoints(); ++j){ Link* joint = ikPath->joint(j); qTranslated[joint->jointId] = joint->q; } } } if(motion->hasRelativeZmpSeq()){ const Vector3& relZmp = motion->relativeZmpSeq()->at(frame); zmp_.noalias() = waist.linear() * relZmp + waist.translation(); // original world position } return true; } bool BodyMotionPoseProvider::seek(double time) { return seek(time, 0, Vector3::Zero(), false); } bool BodyMotionPoseProvider::seek(double time, int waistLinkIndex, const Vector3& waistTranslation) { return seek(time, waistLinkIndex, waistTranslation, true); } int BodyMotionPoseProvider::baseLinkIndex() const { return 0; } bool BodyMotionPoseProvider::getBaseLinkPosition(Vector3& out_p, Matrix3& out_R) const { out_p = p_waist; out_R = R_waist; return true; } void BodyMotionPoseProvider::getJointPositions(std::vector< boost::optional >& out_q) const { int n = body_->numJoints(); out_q.resize(n); for(int i=0; i < n; ++i){ out_q[i] = qTranslated[i]; } } boost::optional BodyMotionPoseProvider::zmp() const { return zmp_; } choreonoid-1.1.0+dfsg/src/Body/BodyMotionPoseProvider.h000066400000000000000000000030341207742442300230500ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_BODY_MOTION_POSE_PROVIDER_H_INCLUDED #define CNOID_BODY_BODY_MOTION_POSE_PROVIDER_H_INCLUDED #include "Body.h" #include "Link.h" #include "PoseProvider.h" #include "BodyMotion.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT BodyMotionPoseProvider : public PoseProvider { public: BodyMotionPoseProvider(); BodyMotionPoseProvider(BodyPtr body, BodyMotionPtr motion); void initialize(BodyPtr body, BodyMotionPtr motion); bool updateMotion(); virtual Body* body() const; virtual double beginningTime() const; virtual double endingTime() const; virtual bool seek(double time); virtual bool seek(double time, int waistLinkIndex, const Vector3& waistTranslation); virtual int baseLinkIndex() const; virtual bool getBaseLinkPosition(Vector3& out_p, Matrix3& out_R) const; virtual void getJointPositions(std::vector< boost::optional >& out_q) const; virtual boost::optional zmp() const; private: BodyPtr body_; BodyMotionPtr motion; int minNumJoints; std::vector footLinks; std::vector ikPaths; MultiAffine3SeqPtr footLinkPositions; std::vector qTranslated; Vector3 p_waist; Matrix3 R_waist; Vector3 zmp_; bool seek(double time, int waistLinkIndex, const Vector3& waistTranslation, bool applyWaistTranslation); }; } #endif choreonoid-1.1.0+dfsg/src/Body/BodyMotionUtil.cpp000066400000000000000000000305671207742442300217120ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "BodyMotionUtil.h" #include "BodyMotion.h" #include "Link.h" #include "Sensor.h" #include #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; static bool saveRootLinkAttAsRpyFormat(BodyMotion& motion, const std::string& filename, std::ostream& os) { format f("%1$.4f %2$g %3$g %4$g\n"); const MultiAffine3SeqPtr& linkPosSeq = motion.linkPosSeq(); const int nFrames = linkPosSeq->numFrames(); if(nFrames > 0 && linkPosSeq->numParts() > 0){ ofstream ofs(filename.c_str()); if(!ofs){ os << filename + " cannot be opened."; return false; } const double r = linkPosSeq->frameRate(); MultiAffine3Seq::View root = linkPosSeq->part(0); for(int i=0; i < nFrames; ++i){ Vector3 rpy(rpyFromRot(root[i].linear())); for(int j=0; j < 3; ++j){ if(fabs(rpy[j]) < 1.0e-14){ rpy[j] = 0.0; } } ofs << (f % (i / r) % rpy[0] % rpy[1] % rpy[2]); } return true; } return false; } static bool saveRootLinkAccAsGsensFile(BodyMotion& motion, BodyPtr body, const std::string& filename, std::ostream& os) { if(!body){ return false; } format f("%1$.4f %2$g %3$g %4$g\n"); const MultiAffine3SeqPtr& linkPosSeq = motion.linkPosSeq(); Sensor* gsens = 0; if(body->numSensors(Sensor::ACCELERATION) > 0){ gsens = body->sensor(Sensor::ACCELERATION, 0); } if(!gsens || !gsens->link || gsens->link->index >= linkPosSeq->numParts()){ return false; } const int nFrames = linkPosSeq->numFrames(); if(nFrames > 0){ ofstream ofs(filename.c_str()); ofs.setf(ios::fixed); if(!ofs){ os << filename + " cannot be opened."; return false; } Vector3Seq accSeq; calcLinkAccSeq(*linkPosSeq, gsens, 0, nFrames, accSeq); const double r = linkPosSeq->frameRate(); for(int i=0; i < nFrames; ++i){ Vector3 a = accSeq[i]; for(int j=0; j < 3; ++j){ if(fabs(a[j]) < 1.0e-14){ a[j] = 0.0; } } ofs << (f % (i / r) % a[0] % a[1] % a[2]); } return true; } return false; } void cnoid::calcLinkAccSeq (MultiAffine3Seq& linkPosSeq, Sensor* gsens, int frameBegin, int numFrames, Vector3Seq& out_accSeq) { const double r = linkPosSeq.frameRate(); const double dt = 1.0 / r; vector vseq(numFrames); MultiAffine3Seq::View pseq = linkPosSeq.part(gsens->link->index); int last = numFrames - 1; for(int i=0; i < last; ++i){ const Affine3& P0 = pseq[frameBegin + i]; const Affine3& P1 = pseq[frameBegin + i + 1]; Vector3 w = omegaFromRot(P0.linear().transpose() * P1.linear()) / dt; vseq[i].noalias() = (P1.translation() - P0.translation()) / dt + P0.linear() * w.cross(gsens->localPos); } vseq[last].setZero(); out_accSeq.setNumFrames(numFrames); out_accSeq[0].noalias() = (pseq[frameBegin].linear().transpose() * vseq[0]) / dt; for(int i=1; i < numFrames; ++i){ out_accSeq[i].noalias() = pseq[frameBegin + i].linear().transpose() * (vseq[i] - vseq[i-1]) / dt; } } bool cnoid::loadHrpsysSeqFileSet(BodyMotion& motion, const std::string& filename, std::ostream& os) { motion.setNumFrames(0); filesystem::path orgpath(filename); bool loaded = false; filesystem::path posFile = filesystem::change_extension(orgpath, ".pos"); if(filesystem::exists(posFile) && !filesystem::is_directory(posFile)){ string posFileString(posFile.file_string()); if(motion.jointPosSeq()->loadPlainFormat(posFileString)){ if(posFileString == filename){ loaded = true; } } } filesystem::path waistFile = filesystem::change_extension(orgpath, ".waist"); if(filesystem::exists(waistFile) && !filesystem::is_directory(waistFile)){ string waistFileString(waistFile.file_string()); if(motion.linkPosSeq()->loadPlainFormat(waistFileString)){ if(waistFileString == filename){ loaded = true; } } } filesystem::path zmpFile = filesystem::change_extension(orgpath, ".zmp"); if(filesystem::exists(zmpFile) && !filesystem::is_directory(zmpFile)){ string zmpFileString(zmpFile.file_string()); if(motion.relativeZmpSeq()->loadPlainFormat(zmpFileString)){ if(zmpFileString == filename){ loaded = true; } } } if(!loaded){ motion.setNumFrames(0); } return loaded; } bool cnoid::saveHrpsysSeqFileSet(BodyMotion& motion, BodyPtr body, const std::string& filename, std::ostream& os) { filesystem::path orgpath(filename); filesystem::path bpath(orgpath.branch_path() / filesystem::path(basename(orgpath))); if(motion.jointPosSeq()->saveAsPlainFormat(filesystem::change_extension(orgpath, ".pos").file_string()) && motion.linkPosSeq()->saveTopPartAsPlainFormat(filesystem::change_extension(orgpath, ".waist").file_string()) && saveRootLinkAttAsRpyFormat(motion, filesystem::change_extension(orgpath, ".hip").file_string(), os)){ saveRootLinkAccAsGsensFile(motion, body, filesystem::change_extension(orgpath, ".gsens").file_string(), os); if(motion.hasRelativeZmpSeq()){ return motion.relativeZmpSeq()->saveAsPlainFormat(filesystem::change_extension(orgpath, ".zmp").file_string()); } return true; } return false; } static void applyPollardVelocityLimitFilterSub (MultiValueSeq::View seq, double deltaUVLimit, double deltaLVLimit, double deltaKs, vector& forward, vector& backward) { const double sqrtDeltaKs = sqrt(deltaKs); const int n = seq.size(); const int last = n - 1; double v0, vv0, vv1; double aa1; // forward pass forward[0] = seq[0]; v0 = 0.0; vv0 = 0.0; for(int i = 0; i < last; ++i){ aa1 = 2.0 * sqrtDeltaKs * (v0 - vv0) + deltaKs * (seq[i] - forward[i]); vv1 = vv0 + aa1; if(vv1 > deltaUVLimit){ vv1 = deltaUVLimit; } else if(vv1 < deltaLVLimit) { vv1 = deltaLVLimit; } forward[i+1] = forward[i] + vv1; v0 = seq[i+1] - seq[i]; vv0 = vv1; } // backward pass v0 = 0.0; vv0 = 0.0; backward[last] = seq[last]; for(int i = last; i > 0; --i) { aa1 = 2.0 * sqrtDeltaKs * (v0 - vv0) + deltaKs * (seq[i] - backward[i]); vv1 = vv0 + aa1; if(vv1 > -deltaLVLimit){ vv1 = -deltaLVLimit; } else if(vv1 < -deltaUVLimit){ vv1 = -deltaUVLimit; } backward[i-1] = backward[i] + vv1; v0 = seq[i-1] - seq[i]; vv0 = vv1; } // average the forward and backward passes for(int i=0; i < n; i++){ seq[i] = 0.5 * (forward[i] + backward[i]); } } static void applyVelocityLimitFilterSub (MultiValueSeq::View seq, double deltaUVLimit, double deltaLVLimit, vector& forward, vector& backward) { const int n = seq.size(); const int last = n - 1; // forward pass forward[0] = seq[0]; for(int i = 0; i < last; ++i){ double v = (seq[i+1] - forward[i]); if(v > deltaUVLimit){ v = deltaUVLimit; } else if(v < deltaLVLimit){ v = deltaLVLimit; } forward[i+1] = forward[i] + v; } // backward pass backward[last] = seq[last]; for(int i = last; i > 0; --i) { double v = (seq[i-1] - backward[i]); if(v > -deltaLVLimit){ v = -deltaLVLimit; } else if(v < -deltaUVLimit){ v = -deltaUVLimit; } backward[i-1] = backward[i] + v; } // average the forward and backward passes for(int i=0; i < n; i++){ seq[i] = 0.5 * (forward[i] + backward[i]); } } bool cnoid::applyVelocityLimitFilter2(MultiValueSeq& seq, int part, double absLimit) { cout << "applyVelocityLimitFilter(seq," << part << ", " << absLimit << ")" << endl; const int numFrames = seq.numFrames(); const double frameRate = seq.frameRate(); vector forward(numFrames); vector backward(numFrames); const double deltaUVLimit = absLimit / frameRate; const double deltaLVLimit = -absLimit / frameRate; applyVelocityLimitFilterSub(seq.part(part), deltaUVLimit, deltaLVLimit, forward, backward); return true; } bool cnoid::applyVelocityLimitFilterDummy() { cout << "applyVelocityLimitFilterDummy()" << endl; return false; } static bool applyVelocityLimitFilterMain (MultiValueSeq& seq, BodyPtr body, double ks, bool usePollardMethod, std::ostream& os) { bool applied = false; os << "applying the velocity limit filter ..." << endl; const int numParts = seq.numParts(); const int numFrames = seq.numFrames(); const double frameRate = seq.frameRate(); const double deltaKs = ks / frameRate; vector forward(numFrames); vector backward(numFrames); int n = std::min(numParts, body->numJoints()); for(int i=0; i < n; ++i){ Link* joint = body->joint(i); if(joint->uvlimit != std::numeric_limits::max() || joint->lvlimit != -std::numeric_limits::max()){ const double deltaUVLimit = joint->uvlimit / frameRate; const double deltaLVLimit = joint->lvlimit / frameRate; os << str(format(" seq %1%: lower limit = %2%, upper limit = %3%") % i % joint->lvlimit % joint->uvlimit) << endl; if(usePollardMethod){ applyPollardVelocityLimitFilterSub( seq.part(i), deltaUVLimit, deltaLVLimit, deltaKs, forward, backward); } else { applyVelocityLimitFilterSub( seq.part(i), deltaUVLimit, deltaLVLimit, forward, backward); } applied = true; } } return applied; } bool cnoid::applyPollardVelocityLimitFilter (MultiValueSeq& seq, BodyPtr body, double ks, std::ostream& os) { return applyVelocityLimitFilterMain(seq, body, ks, true, os); } bool cnoid::applyVelocityLimitFilter (MultiValueSeq& seq, BodyPtr body, std::ostream& os) { return applyVelocityLimitFilterMain(seq, body, 1.0, false, os); } void cnoid::applyGaussianFilter (MultiValueSeq& seq, double sigma, int range, std::ostream& os) { vector gwin; setGaussWindow(sigma, range, gwin); vector orgseq(seq.numFrames()); for(int i=0; i < seq.numParts(); ++i){ if(i==0){ os << str(format("applying the gaussian filter (sigma = %1%, range = %2%) to seq") % sigma % range) << endl; } os << " " << i; MultiValueSeq::View part = seq.part(i); std::copy(part.begin(), part.end(), orgseq.begin()); applyGaussianFilter(part, orgseq, gwin, 0.0); } } void cnoid::applyRangeLimitFilter (MultiValueSeq& seq, BodyPtr body, double limitGrad, double edgeGradRatio, double margin, std::ostream& os) { RangeLimiter limiter; os << "applying the joint position range limit filter ..." << endl; const int numParts = seq.numParts(); int n = std::min(numParts, body->numJoints()); for(int i=0; i < n; ++i){ Link* joint = body->joint(i); if(joint->ulimit != std::numeric_limits::max() || joint->llimit != -std::numeric_limits::max()){ const double ulimit = joint->ulimit - margin; const double llimit = joint->llimit + margin; if(ulimit > llimit){ os << str(format(" seq %1%: lower limit = %2%, upper limit = %3%") % i % joint->llimit % joint->ulimit) << endl; MultiValueSeq::View part = seq.part(i); limiter.apply(part, ulimit, llimit, limitGrad, edgeGradRatio); } } } } choreonoid-1.1.0+dfsg/src/Body/BodyMotionUtil.h000066400000000000000000000027771207742442300213610ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_BODY_MOTION_UTIL_H_INCLUDED #define CNOID_BODY_BODY_MOTION_UTIL_H_INCLUDED #include "Body.h" #include #include #include #include "exportdecl.h" namespace cnoid { class BodyMotion; class Vector3Seq; class MultiAffine3Seq; class MultiValueSeq; class PoseProvider; CNOID_EXPORT bool loadHrpsysSeqFileSet( BodyMotion& motion, const std::string& filename, std::ostream& os); CNOID_EXPORT bool saveHrpsysSeqFileSet( BodyMotion& motion, BodyPtr body, const std::string& filename, std::ostream& os); CNOID_EXPORT void calcLinkAccSeq( MultiAffine3Seq& linkPosSeq, Sensor* gsens, int frameBegin, int numFrames, Vector3Seq& out_accSeq); CNOID_EXPORT bool applyVelocityLimitFilter( MultiValueSeq& seq, BodyPtr body, std::ostream& os = nullout()); CNOID_EXPORT bool applyVelocityLimitFilter2(MultiValueSeq& seq, int part, double absLimit); CNOID_EXPORT bool applyVelocityLimitFilterDummy(); CNOID_EXPORT bool applyPollardVelocityLimitFilter( MultiValueSeq& seq, BodyPtr body, double ks, std::ostream& os = nullout()); CNOID_EXPORT void applyGaussianFilter( MultiValueSeq& seq, double sigma, int range, std::ostream& os = nullout()); CNOID_EXPORT void applyRangeLimitFilter( MultiValueSeq& seq, BodyPtr body, double limitGrad, double edgeGradRatio, double margin, std::ostream& os = nullout()); } #endif choreonoid-1.1.0+dfsg/src/Body/BodySymmetry.cpp000066400000000000000000000136061207742442300214330ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "BodySymmetry.h" #include using namespace std; using namespace cnoid; BodySymmetry::BodySymmetry(BodyPtr body) : body(body) { const YamlSequence& sjoints = *body->info()->findSequence("symmetricJoints"); if(sjoints.isValid() && !sjoints.empty()){ for(int i=0; i < sjoints.size(); ++i){ const YamlSequence& jointPair = *sjoints[i].toSequence(); if(jointPair.size() == 1){ Link* link = body->link(jointPair[0].toString()); if(link){ JointSymmetry& symmetry = jointSymmetryMap[link->jointId]; symmetry.counterPartJointId = link->jointId; symmetry.jointPositionSign = -1.0; symmetry.offset = 0.0; } } else if(jointPair.size() >= 2){ Link* joint[2]; joint[0] = body->link(jointPair[0].toString()); joint[1] = body->link(jointPair[1].toString()); if(joint[0] && joint[1] && joint[0]->jointId >= 0 && joint[1]->jointId >= 0){ for(int j=0; j < 2; ++j){ int other = 1 - j; JointSymmetry& symmetry = jointSymmetryMap[joint[j]->jointId]; symmetry.counterPartJointId = joint[other]->jointId; symmetry.sign = (jointPair.size() >= 3) ? jointPair[2].toDouble() : 1.0; symmetry.offset = (jointPair.size() >= 4) ? jointPair[3].toDouble() : 0.0; } } } } } const YamlSequence& slinks = *body->info()->findSequence("symmetricIkLinks"); if(slinks.isValid() && !slinks.empty()){ for(int i=0; i < slinks.size(); ++i){ const YamlSequence& linkPair = *slinks[i].toSequence(); if(linkPair.size() == 1){ Link* link = body->link(linkPair[0].toString()); if(link){ LinkSymmetry& symmetry = linkFlipMap[link->index]; symmetry.counterPartIndex = link->index; } } else if(linkPair.size() >= 2){ Link* link[2]; link[0] = body->link(linkPair[0].toString()); link[1] = body->link(linkPair[1].toString()); if(link[0] && link[1]){ for(int j=0; j < 2; ++j){ int other = 1 - j; LinkSymmetry& symmetry = linkFlipMap[link[j]->index]; symmetry.counterPartIndex = link[other]->index; } } } } } } void SymmetricPoseChanger::apply(OperationType op) { JointSymmetryMap::iterator p = jointSymmetryMap.begin(); while(p != jointSymmetryMap.end()){ ++p; } } bool modified = false; pose->setNumJoints(0); for(int i=0; i < orgPose->numJoints(); ++i){ if(orgPose->isJointValid(i)){ double q = orgPose->jointPosition(i); JointFlipInfoMap::iterator p = jointFlipInfoMap.find(i); if(p == jointFlipInfoMap.end()){ pose->setJointPosition(i, q); } else { JointFlipInfo& flip = p->second; pose->setJointPosition(flip.counterPartJointId, q * flip.jointPositionSign); modified = true; } } } pose->clearIkLinks(); for(Pose::LinkInfoMap::iterator p = orgPose->ikLinkBegin(); p != orgPose->ikLinkEnd(); ++p){ int index = p->first; Pose::LinkInfo& orgInfo = p->second; Pose::LinkInfo* info; LinkFlipMap::iterator q = linkFlipMap.find(index); if(q != linkFlipMap.end()){ index = q->second; info = pose->addIkLink(index); info->p = orgInfo.p; info->p.y() = -info->p.y(); Matrix3& R = orgInfo.R; info->R << R(0, 0), -R(0, 1), R(0, 2), -R(1, 0), R(1, 1), -R(1, 2), R(2, 0), -R(2, 1), R(2, 2); modified = true; } else { info = pose->addIkLink(index); info->p = orgInfo.p; info->R = orgInfo.R; } info->setStationaryPoint(orgInfo.isStationaryPoint()); if(orgInfo.isTouching()){ info->setTouching(orgInfo.partingDirection()); } info->setSlave(orgInfo.isSlave()); if(orgInfo.isBaseLink()){ pose->setBaseLink(index); } } if(orgPose->isZmpValid()){ Vector3 zmp = orgPose->zmp(); zmp.y() = -zmp.y(); pose->setZmp(zmp); pose->setZmpStationaryPoint(orgPose->isZmpStationaryPoint()); modified = true; } return modified; } void cnoid::flipPoses(PoseSeqPtr seq, BodyPtr body) { FlipFilter filiter(body); filiter.flip(seq); } void cnoid::rotateYawOrientations (PoseSeqPtr seq, PoseSeq::iterator begin, const Vector3& center, double angle) { const Matrix3 Rz(AngleAxisd(angle, Vector3::UnitZ())); PoseSeq::iterator poseIter; for(poseIter = begin; poseIter != seq->end(); ++poseIter){ PosePtr pose = poseIter->get(); if(pose){ if(pose->numIkLinks() > 0 || pose->isZmpValid()){ seq->beginPoseModification(poseIter); for(Pose::LinkInfoMap::iterator p = pose->ikLinkBegin(); p != pose->ikLinkEnd(); ++p){ Pose::LinkInfo& linkInfo = p->second; linkInfo.p = Rz * (linkInfo.p - center) + center; linkInfo.R = Rz * linkInfo.R; } if(pose->isZmpValid()){ pose->setZmp(Rz * (pose->zmp() - center) + center); } seq->endPoseModification(poseIter); } } } } choreonoid-1.1.0+dfsg/src/Body/BodySymmetry.h000066400000000000000000000015531207742442300210760ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_BODY_SYMMETRY_H_INCLUDED #define CNOID_BODY_BODY_SYMMETRY_H_INCLUDED #include "Body.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT SymmetricPoseChanger { public: SymmetricPoseChanger(BodyPtr body); enum OperationType { LEFT_TO_RIGHT, RIGHT_TO_LEFT, FLIP }; void apply(OperationType op); private: BodyPtr body; struct JointSymmetry { int counterPartId; double sign; double offset; }; typedef std::map JointSymmetryMap; JointSymmetryMap jointSymmetryMap; struct LinkSymmetry { int counterPartIndex; }; typedef std::map LinkSymmetryMap; LinkSymmetryMap linkSymmetryMap; }; } #endif choreonoid-1.1.0+dfsg/src/Body/CMakeLists.txt000066400000000000000000000043301207742442300210120ustar00rootroot00000000000000 # @author Shin'ichiro Nakaoka set(target CnoidBody) set(sources Body.cpp LeggedBody.cpp BodyCustomizerInterface.cpp BodyLoader.cpp ModelNodeSet.cpp Link.cpp LinkTraverse.cpp LinkPath.cpp JointPath.cpp LinkGroup.cpp ColdetLinkPair.cpp Sensor.cpp CompositeIK.cpp PinDragIK.cpp PenetrationBlocker.cpp World.cpp ForwardDynamics.cpp ForwardDynamicsABM.cpp ForwardDynamicsCBM.cpp InverseDynamics.cpp Jacobian.cpp MassMatrix.cpp ConstraintForceSolver.cpp BodyMotion.cpp BodyMotionPoseProvider.cpp PoseProviderToBodyMotionConverter.cpp BodyMotionUtil.cpp ) set(headers Body.h LeggedBody.h BodyCustomizerInterface.h BodyLoader.h ModelNodeSet.h Link.h LinkTraverse.h LinkPath.h JointPath.h LinkGroup.h ColdetLinkPair.h Sensor.h InverseKinematics.h CompositeIK.h PinDragIK.h PenetrationBlocker.h World.h ForwardDynamics.h ForwardDynamicsABM.h ForwardDynamicsCBM.h InverseDynamics.h Jacobian.h MassMatrix.h ConstraintForceSolver.h PoseProvider.h BodyMotion.h BodyMotionPoseProvider.h PoseProviderToBodyMotionConverter.h BodyMotionUtil.h exportdecl.h ) add_library(${target} SHARED ${sources} ${headers}) if(UNIX) target_link_libraries(${target} CnoidUtil CnoidCollision dl) elseif(MSVC) target_link_libraries(${target} CnoidUtil CnoidCollision) endif() apply_common_setting_for_library(${target} "${headers}") # Body Customizers set(BODY_CUSTOMIZERS ${BODY_CUSTOMIZERS} CACHE FILEPATH "Source files of body customizers") if(BODY_CUSTOMIZERS) foreach(src ${BODY_CUSTOMIZERS}) get_filename_component(customizer ${src} NAME_WE) add_library(${customizer} SHARED ${src}) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) set_target_properties(${customizer} PROPERTIES PREFIX "") set_target_properties(${customizer} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CNOID_PLUGIN_SUBDIR}/customizer) install(TARGETS ${customizer} RUNTIME DESTINATION ${CNOID_PLUGIN_SUBDIR}/customizer CONFIGURATIONS Release Debug LIBRARY DESTINATION ${CNOID_PLUGIN_SUBDIR}/customizer CONFIGURATIONS Release Debug) endforeach() endif() install_external_libraries(${Boost_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS}) choreonoid-1.1.0+dfsg/src/Body/ColdetLinkPair.cpp000066400000000000000000000014471207742442300216300ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "ColdetLinkPair.h" namespace cnoid { class Link; ColdetLinkPair::ColdetLinkPair(Link* link1, Link* link2) : ColdetModelPair(link1->coldetModel, link2->coldetModel) { links[0] = link1; links[1] = link2; } ColdetLinkPair::ColdetLinkPair(const ColdetLinkPair& org) : ColdetModelPair(org) { links[0] = org.links[0]; links[1] = org.links[1]; } ColdetLinkPair::~ColdetLinkPair() { } void ColdetLinkPair::updatePositions() { model(0)->setPosition(links[0]->R, links[0]->p); model(1)->setPosition(links[1]->R, links[1]->p); } Link* ColdetLinkPair::link(int index) { return links[index]; } } choreonoid-1.1.0+dfsg/src/Body/ColdetLinkPair.h000066400000000000000000000013471207742442300212740ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_COLDET_LINK_PAIR_H_INCLUDED #define CNOID_BODY_COLDET_LINK_PAIR_H_INCLUDED #include "Link.h" #include #include "exportdecl.h" namespace cnoid { class Link; class CNOID_EXPORT ColdetLinkPair : public ColdetModelPair { public: ColdetLinkPair(Link* link1, Link* link2); ColdetLinkPair(const ColdetLinkPair& org); virtual ~ColdetLinkPair(); void updatePositions(); Link* link(int index); protected: Link* links[2]; private: }; typedef boost::intrusive_ptr ColdetLinkPairPtr; } #endif choreonoid-1.1.0+dfsg/src/Body/CompositeIK.cpp000066400000000000000000000043111207742442300211430ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "CompositeIK.h" #include "Link.h" #include "JointPath.h" using namespace std; using namespace boost; using namespace cnoid; CompositeIK::CompositeIK(BodyPtr body, Link* targetLink) { body_ = body; targetLink_ = targetLink; isAnalytical_ = false; } CompositeIK::~CompositeIK() { } bool CompositeIK::addBaseLink(Link* baseLink) { if(baseLink && targetLink_){ JointPathPtr path = body_->getJointPath(targetLink_, baseLink); if(path){ isAnalytical_ = pathList.empty() ? path->hasAnalyticalIK() : (isAnalytical_ && path->hasAnalyticalIK()); PathInfo info; info.path = path; info.p0 = baseLink->p; info.R0 = baseLink->R; pathList.push_back(info); return true; } } return false; } void CompositeIK::setMaxIKerror(double e) { for(size_t i=0; i < pathList.size(); ++i){ pathList[i].path->setMaxIKerror(e); } } bool CompositeIK::hasAnalyticalIK() const { return isAnalytical_; } bool initialize(); bool CompositeIK::calcInverseKinematics(const Vector3& p, const Matrix3& R) { const int n = body_->numJoints(); Vector3 p0 = targetLink_->p; Matrix3 R0 = targetLink_->R; std::vector q0(n); for(int i=0; i < n; ++i){ q0[i] = body_->joint(i)->q; } targetLink_->p = p; targetLink_->R = R; bool solved = true; for(size_t i=0; i < pathList.size(); ++i){ PathInfo& info = pathList[i]; solved = info.path->calcInverseKinematics(p, R, info.p0, info.R0); if(!solved){ break; } Link* endLink = info.path->endLink(); endLink->p = info.p0; endLink->R = info.R0; } if(!solved){ targetLink_->p = p0; targetLink_->R = R0; for(int i=0; i < n; ++i){ body_->joint(i)->q = q0[i]; } for(size_t i=0; i < pathList.size(); ++i){ PathInfo& info = pathList[i]; info.path->calcForwardKinematics(); Link* endLink = info.path->endLink(); endLink->p = info.p0; endLink->R = info.R0; } } return solved; } choreonoid-1.1.0+dfsg/src/Body/CompositeIK.h000066400000000000000000000017771207742442300206250ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_COMPOSITE_IK_H_INCLUDED #define CNOID_BODY_COMPOSITE_IK_H_INCLUDED #include "Body.h" #include "InverseKinematics.h" #include #include "exportdecl.h" namespace cnoid { class Link; class CNOID_EXPORT CompositeIK : public InverseKinematics { public: CompositeIK(BodyPtr body, Link* targetLink); ~CompositeIK(); BodyPtr body() const { return body_; } bool addBaseLink(Link* link); void setMaxIKerror(double e); virtual bool hasAnalyticalIK() const; virtual bool calcInverseKinematics(const Vector3& p, const Matrix3& R); private: BodyPtr body_; Link* targetLink_; struct PathInfo { JointPathPtr path; Vector3 p0; Matrix3 R0; }; std::vector pathList; bool isAnalytical_; }; typedef boost::shared_ptr CompositeIKptr; } #endif choreonoid-1.1.0+dfsg/src/Body/ConstraintForceSolver.cpp000066400000000000000000002063311207742442300232610ustar00rootroot00000000000000/** \file \brief Implementation of ConstraintForceSolver class \author Shin'ichiro Nakaoka */ #ifdef __WIN32__ #define NOMINMAX #endif #include "World.h" #include "Body.h" #include "Link.h" #include "LinkTraverse.h" #include "ForwardDynamicsCBM.h" #include "ConstraintForceSolver.h" #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; // Is LCP solved by Iterative or Pivoting method ? // #define USE_PIVOTING_LCP #ifdef USE_PIVOTING_LCP #include "SimpleLCP_Path.h" static const bool usePivotingLCP = true; #else static const bool usePivotingLCP = false; #endif // settings static const double VEL_THRESH_OF_DYNAMIC_FRICTION = 1.0e-4; static const bool ENABLE_STATIC_FRICTION = true; static const bool ONLY_STATIC_FRICTION_FORMULATION = (true && ENABLE_STATIC_FRICTION); static const bool STATIC_FRICTION_BY_TWO_CONSTRAINTS = true; static const bool IGNORE_CURRENT_VELOCITY_IN_STATIC_FRICTION = false; static const bool ENABLE_TRUE_FRICTION_CONE = (true && ONLY_STATIC_FRICTION_FORMULATION && STATIC_FRICTION_BY_TWO_CONSTRAINTS); static const bool SKIP_REDUNDANT_ACCEL_CALC = true; static const bool ASSUME_SYMMETRIC_MATRIX = false; static const int DEFAULT_MAX_NUM_GAUSS_SEIDEL_ITERATION = 500; static const int DEFAULT_NUM_GAUSS_SEIDEL_ITERATION_BLOCK = 10; static const int DEFAULT_NUM_GAUSS_SEIDEL_INITIAL_ITERATION = 0; static const double DEFAULT_GAUSS_SEIDEL_MAX_REL_ERROR = 1.0e-3; static const bool USE_PREVIOUS_LCP_SOLUTION = true; static const bool ALLOW_SUBTLE_PENETRATION_FOR_STABILITY = true; // normal setting static const double ALLOWED_PENETRATION_DEPTH = 0.0001; //static const double PENETRATION_A = 500.0; //static const double PENETRATION_B = 80.0; static const double DEFAULT_NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION = 10.0; // test for mobile robots with wheels //static const double ALLOWED_PENETRATION_DEPTH = 0.005; //static const double PENETRATION_A = 500.0; //static const double PENETRATION_B = 80.0; //static const double NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION = 10.0; //static const bool ENABLE_CONTACT_POINT_THINNING = false; // experimental options static const bool PROPORTIONAL_DYNAMIC_FRICTION = false; static const bool ENABLE_RANDOM_STATIC_FRICTION_BASE = false; // debug options static const bool CFS_DEBUG = false; static const bool CFS_DEBUG_VERBOSE = false; static const bool CFS_DEBUG_VERBOSE_2 = false; static const bool CFS_DEBUG_VERBOSE_3 = false; static const bool CFS_DEBUG_LCPCHECK = false; static const bool CFS_MCP_DEBUG = false; static const bool CFS_MCP_DEBUG_SHOW_ITERATION_STOP = false; static const bool CFS_PUT_NUM_CONTACT_POINTS = false; namespace cnoid { class CFSImpl { public: CFSImpl(WorldBase& world); bool addCollisionCheckLinkPair (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double epsilon); void initialize(void); void solve(); inline void clearExternalForces(); WorldBase& world; bool isConstraintForceOutputMode; struct ConstraintPoint { int globalIndex; Vector3 point; Vector3 normalTowardInside[2]; Vector3 defaultAccel[2]; double normalProjectionOfRelVelocityOn0; double depth; // position error in the case of a connection point double mu; Vector3 relVelocityOn0; int globalFrictionIndex; int numFrictionVectors; Vector3 frictionVector[4][2]; }; typedef std::vector ConstraintPointArray; struct LinkData { Vector3 dvo; Vector3 dw; Vector3 pf0; Vector3 ptau0; double uu; double uu0; double ddq; int numberToCheckAccelCalcSkip; int parentIndex; Link* link; }; typedef std::vector LinkDataArray; struct BodyData { BodyPtr body; bool isStatic; bool hasConstrainedLinks; bool isTestForceBeingApplied; LinkDataArray linksData; Vector3 dpf; Vector3 dptau; /// only used by the ForwardDynamicsMM mode Vector3* rootLinkPosRef; /** If the body includes high-gain mode joints, the ForwardDynamisMM object of the body is set to this pointer. The pointer is null when all the joints are torque mode and the forward dynamics is calculated by ABM. */ ForwardDynamicsMMPtr forwardDynamicsMM; }; std::vector bodiesData; class LinkPair : public ColdetModelPair { public: int index; bool isSameBodyPair; int bodyIndex[2]; BodyData* bodyData[2]; Link* link[2]; LinkData* linkData[2]; ConstraintPointArray constraintPoints; double muStatic; double muDynamic; double culling_thresh; double epsilon; Body::LinkConnection* connection; }; typedef intrusive_ptr LinkPairPtr; typedef std::vector LinkPairArray; LinkPairArray collisionCheckLinkPairs; LinkPairArray connectedLinkPairs; std::vector constrainedLinkPairs; /** globalNumConstraintVectors = globalNumContactNormalVectors + globalNumConnectionVectors */ int globalNumConstraintVectors; int globalNumContactNormalVectors; int globalNumConnectionVectors; int globalNumFrictionVectors; int prevGlobalNumConstraintVectors; int prevGlobalNumFrictionVectors; bool areThereImpacts; int numUnconverged; typedef Eigen::Matrix MatrixX; typedef VectorXd VectorX; // Mlcp * solution + b _|_ solution MatrixX Mlcp; // constant acceleration term when no external force is applied VectorX an0; VectorX at0; // constant vector of LCP VectorX b; // contact force solution: normal forces at contact points VectorX solution; // random number generator variate_generator > randomAngle; // for special version of gauss sidel iterative solver std::vector frictionIndexToContactIndex; VectorX contactIndexToMu; VectorX mcpHi; int maxNumGaussSeidelIteration; int numGaussSeidelInitialIteration; double gaussSeidelMaxRelError; double negativeVelocityRatioForPenetration; int numGaussSeidelTotalLoops; int numGaussSeidelTotalCalls; void setConstraintPoints(); bool setContactConstraintPoint(LinkPair& linkPair, collision_data& cd, int indexInCd); void setFrictionVectors(ConstraintPoint& constraintPoint); bool setConnectionConstraintPoints(LinkPair& linkPair); void putContactPoints(); void solveImpactConstraints(); void initMatrices(); void setAccelCalcSkipInformation(); void setDefaultAccelerationVector(); void setAccelerationMatrix(); void initABMForceElementsWithNoExtForce(BodyData& bodyData); void calcABMForceElementsWithTestForce(BodyData& bodyData, Link* linkToApplyForce, const Vector3& f, const Vector3& tau); void calcAccelsABM(BodyData& bodyData, int constraintIndex); void calcAccelsMM(BodyData& bodyData, int constraintIndex); void extractRelAccelsOfConstraintPoints (Eigen::Block& Kxn, Eigen::Block& Kxt, int testForceIndex, int constraintIndex); void extractRelAccelsFromLinkPairCase1 (Eigen::Block& Kxn, Eigen::Block& Kxt, LinkPair& linkPair, int testForceIndex, int constraintIndex); void extractRelAccelsFromLinkPairCase2 (Eigen::Block& Kxn, Eigen::Block& Kxt, LinkPair& linkPair, int iTestForce, int iDefault, int testForceIndex, int constraintIndex); void extractRelAccelsFromLinkPairCase3 (Eigen::Block& Kxn, Eigen::Block& Kxt, LinkPair& linkPair, int testForceIndex, int constraintIndex); void copySymmetricElementsOfAccelerationMatrix (Eigen::Block& Knn, Eigen::Block& Ktn, Eigen::Block& Knt, Eigen::Block& Ktt); void clearSingularPointConstraintsOfClosedLoopConnections(); void setConstantVectorAndMuBlock(); void addConstraintForceToLinks(); void addConstraintForceToLink(LinkPair* linkPair, int ipair); void solveMCPByProjectedGaussSeidel (const MatrixX& M, const VectorX& b, VectorX& x); void solveMCPByProjectedGaussSeidelInitial (const MatrixX& M, const VectorX& b, VectorX& x, const int numIteration); void solveMCPByProjectedGaussSeidelMain (const MatrixX& M, const VectorX& b, VectorX& x, const int numIteration); double solveMCPByProjectedGaussSeidelErrorCheck (const MatrixX& M, const VectorX& b, VectorX& x); void checkLCPResult(MatrixX& M, VectorX& b, VectorX& x); void checkMCPResult(MatrixX& M, VectorX& b, VectorX& x); #ifdef USE_PIVOTING_LCP bool callPathLCPSolver(MatrixX& Mlcp, VectorX& b, VectorX& solution); // for PATH solver std::vector lb; std::vector ub; std::vector m_i; std::vector m_j; std::vector m_ij; #endif }; /* #ifdef _MSC_VER const double CFSImpl::PI = 3.14159265358979323846; const double CFSImpl::PI_2 = 1.57079632679489661923; #endif */ }; template static void putMatrix(TMatrix& M, const char *name) { if(M.cols() == 1){ std::cout << "Vector " << name << M << std::endl; } else { std::cout << "Matrix " << name << ": \n"; for(int i=0; i < M.rows(); i++){ for(int j=0; j < M.cols(); j++){ std::cout << boost::format(" %6.3f ") % M(i, j); } std::cout << std::endl; } } } template static void putVector(const TVector& M, const char *name) { std::cout << "Vector " << name << M << std::endl; } template static inline void debugPutMatrix(const TMatrix& M, const char *name) { if(CFS_DEBUG_VERBOSE) putMatrix(M, name); } template static inline void debugPutVector(const TVector& M, const char *name) { if(CFS_DEBUG_VERBOSE) putVector(M, name); } CFSImpl::CFSImpl(WorldBase& world) : world(world), randomAngle(mt19937(), uniform_real<>(0.0, 2.0 * PI)) { maxNumGaussSeidelIteration = DEFAULT_MAX_NUM_GAUSS_SEIDEL_ITERATION; numGaussSeidelInitialIteration = DEFAULT_NUM_GAUSS_SEIDEL_INITIAL_ITERATION; gaussSeidelMaxRelError = DEFAULT_GAUSS_SEIDEL_MAX_REL_ERROR; negativeVelocityRatioForPenetration = DEFAULT_NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION; isConstraintForceOutputMode = false; } bool CFSImpl::addCollisionCheckLinkPair (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double epsilon) { int index; int isRegistered; tie(index, isRegistered) = world.getIndexOfLinkPairs(link1, link2); if(index >= 0){ int n = collisionCheckLinkPairs.size(); if(index >= n){ collisionCheckLinkPairs.resize(index+1); } LinkPairPtr& linkPair = collisionCheckLinkPairs[index]; if(!linkPair){ linkPair = new LinkPair(); } linkPair->set(link1->coldetModel, link2->coldetModel); linkPair->isSameBodyPair = (bodyIndex1 == bodyIndex2); linkPair->bodyIndex[0] = bodyIndex1; linkPair->link[0] = link1; linkPair->bodyIndex[1] = bodyIndex2; linkPair->link[1] = link2; linkPair->index = index; linkPair->muStatic = muStatic; linkPair->muDynamic = muDynamic; linkPair->culling_thresh = culling_thresh; linkPair->epsilon = epsilon; linkPair->connection = 0; } return (index >= 0 && !isRegistered); } void CFSImpl::initialize(void) { if(CFS_MCP_DEBUG){ numGaussSeidelTotalCalls = 0; numGaussSeidelTotalLoops = 0; } int numBodies = world.numBodies(); bodiesData.resize(numBodies); connectedLinkPairs.clear(); for(int bodyIndex=0; bodyIndex < numBodies; ++bodyIndex){ BodyPtr body = world.body(bodyIndex); body->clearExternalForces(); BodyData& bodyData = bodiesData[bodyIndex]; bodyData.body = body; bodyData.linksData.resize(body->numLinks()); bodyData.hasConstrainedLinks = false; bodyData.isTestForceBeingApplied = false; bodyData.isStatic = body->isStaticModel(); bodyData.forwardDynamicsMM = dynamic_pointer_cast(world.forwardDynamics(bodyIndex)); LinkDataArray& linksData = bodyData.linksData; if(bodyData.isStatic && !(bodyData.forwardDynamicsMM)){ int n = linksData.size(); for(int j=0; j < n; ++j){ LinkData& linkData = linksData[j]; linkData.dw.setZero(); linkData.dvo.setZero(); } } // initialize link data const LinkTraverse& traverse = body->linkTraverse(); for(int j=0; j < traverse.numLinks(); ++j){ Link* link = traverse[j]; linksData[link->index].link = link; linksData[link->index].parentIndex = link->parent ? link->parent->index : -1; } // initialize link connection Body::LinkConnectionArray& connections = body->linkConnections; for(size_t j=0; j < connections.size(); ++j){ connectedLinkPairs.push_back(new LinkPair()); LinkPair& linkPair = *connectedLinkPairs.back(); Body::LinkConnection& connection = connections[j]; linkPair.connection = &connection; linkPair.isSameBodyPair = true; linkPair.constraintPoints.resize(connection.numConstraintAxes); for(int k=0; k < connection.numConstraintAxes; ++k){ ConstraintPoint& constraint = linkPair.constraintPoints[k]; constraint.numFrictionVectors = 0; constraint.globalFrictionIndex = numeric_limits::max(); } for(int k=0; k < 2; ++k){ linkPair.bodyIndex[k] = bodyIndex; linkPair.bodyData[k] = &bodiesData[bodyIndex]; Link* link = connection.link[k]; linkPair.link[k] = link; linkPair.linkData[k] = &(bodyData.linksData[link->index]); } } } int numLinkPairs = collisionCheckLinkPairs.size(); for(int i=0; i < numLinkPairs; ++i){ LinkPair& linkPair = *collisionCheckLinkPairs[i]; for(int j=0; j < 2; ++j){ BodyData& bodyData = bodiesData[linkPair.bodyIndex[j]]; linkPair.bodyData[j] = &bodyData; linkPair.linkData[j] = &(bodyData.linksData[linkPair.link[j]->index]); } } prevGlobalNumConstraintVectors = 0; prevGlobalNumFrictionVectors = 0; numUnconverged = 0; randomAngle.engine().seed(); } inline void CFSImpl::clearExternalForces() { for(size_t i=0; i < bodiesData.size(); ++i){ BodyData& bodyData = bodiesData[i]; if(bodyData.hasConstrainedLinks){ bodyData.body->clearExternalForces(); } } } void CFSImpl::solve() { if(CFS_DEBUG) std::cout << "Time: " << world.currentTime() << std::endl; for(size_t i=0; i < bodiesData.size(); ++i){ bodiesData[i].hasConstrainedLinks = false; bodiesData[i].body->updateLinkColdetModelPositions(); if(isConstraintForceOutputMode){ BodyPtr& body = bodiesData[i].body; const int n = body->numLinks(); for(int j=0; j < n; ++j){ body->link(j)->constraintForces.clear(); } } } globalNumConstraintVectors = 0; globalNumFrictionVectors = 0; areThereImpacts = false; constrainedLinkPairs.clear(); setConstraintPoints(); if(CFS_PUT_NUM_CONTACT_POINTS){ cout << globalNumContactNormalVectors; } if(globalNumConstraintVectors > 0){ if(CFS_DEBUG){ std::cout << "Num Collisions: " << globalNumContactNormalVectors << std::endl; } if(CFS_DEBUG_VERBOSE) putContactPoints(); const bool constraintsSizeChanged = ((globalNumFrictionVectors != prevGlobalNumFrictionVectors) || (globalNumConstraintVectors != prevGlobalNumConstraintVectors)); if(constraintsSizeChanged){ initMatrices(); } if(areThereImpacts){ solveImpactConstraints(); } if(SKIP_REDUNDANT_ACCEL_CALC){ setAccelCalcSkipInformation(); } setDefaultAccelerationVector(); setAccelerationMatrix(); clearSingularPointConstraintsOfClosedLoopConnections(); setConstantVectorAndMuBlock(); if(CFS_DEBUG_VERBOSE){ debugPutVector(an0, "an0"); debugPutVector(at0, "at0"); debugPutMatrix(Mlcp, "Mlcp"); debugPutVector(b.head(globalNumConstraintVectors), "b1"); debugPutVector(b.segment(globalNumConstraintVectors, globalNumConstraintVectors + globalNumFrictionVectors), "b2"); } bool isConverged; #ifdef USE_PIVOTING_LCP isConverged = callPathLCPSolver(Mlcp, b, solution); #else if(!USE_PREVIOUS_LCP_SOLUTION || constraintsSizeChanged){ solution.setZero(); } solveMCPByProjectedGaussSeidel(Mlcp, b, solution); isConverged = true; #endif if(!isConverged){ ++numUnconverged; if(CFS_DEBUG) std::cout << "LCP didn't converge" << numUnconverged << std::endl; } else { if(CFS_DEBUG) std::cout << "LCP converged" << std::endl; if(CFS_DEBUG_LCPCHECK){ // checkLCPResult(Mlcp, b, solution); checkMCPResult(Mlcp, b, solution); } addConstraintForceToLinks(); } } prevGlobalNumConstraintVectors = globalNumConstraintVectors; prevGlobalNumFrictionVectors = globalNumFrictionVectors; } void CFSImpl::setConstraintPoints() { for(size_t colIndex=0; colIndex < collisionCheckLinkPairs.size(); ++colIndex){ LinkPair& linkPair = *collisionCheckLinkPairs[colIndex]; std::vector& cdata = linkPair.detectCollisions(); if(!cdata.empty()){ linkPair.constraintPoints.clear(); bool constraintsAdded = false; for(size_t i=0; i < cdata.size(); ++i){ collision_data& cd = cdata[i]; for(int j=0; j < cd.num_of_i_points; ++j){ if(cd.i_point_new[j]){ if(setContactConstraintPoint(linkPair, cd, j)){ constraintsAdded = true; } } } } if(constraintsAdded){ constrainedLinkPairs.push_back(&linkPair); linkPair.bodyData[0]->hasConstrainedLinks = true; linkPair.bodyData[1]->hasConstrainedLinks = true; } } } globalNumContactNormalVectors = globalNumConstraintVectors; for(size_t i=0; i < connectedLinkPairs.size(); ++i){ LinkPair& linkPair = *connectedLinkPairs[i]; constrainedLinkPairs.push_back(&linkPair); if(setConnectionConstraintPoints(linkPair)){ linkPair.bodyData[0]->hasConstrainedLinks = true; linkPair.bodyData[1]->hasConstrainedLinks = true; } else { constrainedLinkPairs.pop_back(); } } globalNumConnectionVectors = globalNumConstraintVectors - globalNumContactNormalVectors; } /** @retuen true if the point is actually added to the constraints */ bool CFSImpl::setContactConstraintPoint(LinkPair& linkPair, collision_data& cd, int indexInCd) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; constraintPoints.push_back(ConstraintPoint()); ConstraintPoint& contact = constraintPoints.back(); contact.point = cd.i_points[indexInCd]; // dense contact points are eliminated int nPrevPoints = constraintPoints.size() - 1; for(int i=0; i < nPrevPoints; ++i){ if((constraintPoints[i].point - contact.point).norm() < linkPair.culling_thresh){ linkPair.constraintPoints.pop_back(); return false; } } contact.normalTowardInside[1] = cd.n_vector; contact.normalTowardInside[0] = -contact.normalTowardInside[1]; contact.depth = cd.depth; contact.globalIndex = globalNumConstraintVectors++; // check velocities Vector3 v[2]; for(int k=0; k < 2; ++k){ Link* link = linkPair.link[k]; if(link->isRoot() && link->jointType == Link::FIXED_JOINT){ v[k].setZero(); } else { v[k] = link->vo + link->w.cross(contact.point); } } contact.relVelocityOn0 = v[1] - v[0]; contact.normalProjectionOfRelVelocityOn0 = contact.normalTowardInside[1].dot(contact.relVelocityOn0); if( ! areThereImpacts){ if(contact.normalProjectionOfRelVelocityOn0 < -1.0e-6){ areThereImpacts = true; } } Vector3 v_tangent = contact.relVelocityOn0 - contact.normalProjectionOfRelVelocityOn0 * contact.normalTowardInside[1]; contact.globalFrictionIndex = globalNumFrictionVectors; double vt_square = v_tangent.squaredNorm(); static const double vsqrthresh = VEL_THRESH_OF_DYNAMIC_FRICTION * VEL_THRESH_OF_DYNAMIC_FRICTION; bool isSlipping = (vt_square > vsqrthresh); contact.mu = isSlipping ? linkPair.muDynamic : linkPair.muStatic; if( !ONLY_STATIC_FRICTION_FORMULATION && isSlipping){ contact.numFrictionVectors = 1; double vt_mag = sqrt(vt_square); Vector3 t1 = v_tangent / vt_mag; Vector3 t2 = contact.normalTowardInside[1].cross(t1); Vector3 t3 = t2.cross(contact.normalTowardInside[1]); contact.frictionVector[0][0] = t3.normalized(); contact.frictionVector[0][1] = -contact.frictionVector[0][0]; // proportional dynamic friction near zero velocity if(PROPORTIONAL_DYNAMIC_FRICTION){ vt_mag *= 10000.0; if(vt_mag < contact.mu){ contact.mu = vt_mag; } } } else { if(ENABLE_STATIC_FRICTION){ contact.numFrictionVectors = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? 2 : 4); setFrictionVectors(contact); } else { contact.numFrictionVectors = 0; } } globalNumFrictionVectors += contact.numFrictionVectors; return true; } void CFSImpl::setFrictionVectors(ConstraintPoint& contact) { Vector3 u = Vector3::Zero(); int minAxis = 0; Vector3& normal = contact.normalTowardInside[0]; for(int i=1; i < 3; i++){ if(fabs(normal(i)) < fabs(normal(minAxis))){ minAxis = i; } } u(minAxis) = 1.0; Vector3 t1 = normal.cross(u).normalized(); Vector3 t2 = normal.cross(t1).normalized(); if(ENABLE_RANDOM_STATIC_FRICTION_BASE){ double theta = randomAngle(); contact.frictionVector[0][0] = cos(theta) * t1 + sin(theta) * t2; theta += PI_2; contact.frictionVector[1][0] = cos(theta) * t1 + sin(theta) * t2; } else { contact.frictionVector[0][0] = t1; contact.frictionVector[1][0] = t2; } if(STATIC_FRICTION_BY_TWO_CONSTRAINTS){ contact.frictionVector[0][1] = -contact.frictionVector[0][0]; contact.frictionVector[1][1] = -contact.frictionVector[1][0]; } else { contact.frictionVector[2][0] = -contact.frictionVector[0][0]; contact.frictionVector[3][0] = -contact.frictionVector[1][0]; contact.frictionVector[0][1] = contact.frictionVector[2][0]; contact.frictionVector[1][1] = contact.frictionVector[3][0]; contact.frictionVector[2][1] = contact.frictionVector[0][0]; contact.frictionVector[3][1] = contact.frictionVector[1][0]; } } bool CFSImpl::setConnectionConstraintPoints(LinkPair& linkPair) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; Body::LinkConnection* connection = linkPair.connection; Link* link0 = connection->link[0]; Link* link1 = connection->link[1]; Vector3 point[2]; point[0].noalias() = link0->p + link0->R * connection->point[0]; point[1].noalias() = link1->p + link1->R * connection->point[1]; Vector3 midPoint = (point[0] + point[1]) / 2.0; Vector3 error = midPoint - point[0]; if(error.squaredNorm() > (0.04 * 0.04)){ return false; } // check velocities Vector3 v[2]; for(int k=0; k < 2; ++k){ Link* link = connection->link[k]; if(link->isRoot() && link->jointType == Link::FIXED_JOINT){ v[k].setZero(); } else { v[k] = link->vo + link->w.cross(point[k]); } } Vector3 relVelocityOn0 = v[1] - v[0]; for(int i=0; i < connection->numConstraintAxes; ++i){ ConstraintPoint& constraint = constraintPoints[i]; constraint.point = midPoint; const Vector3 axis = link0->R * connection->constraintAxes[i]; constraint.normalTowardInside[0] = axis; constraint.normalTowardInside[1] = -axis; constraint.depth = axis.dot(error); constraint.globalIndex = globalNumConstraintVectors++; constraint.normalProjectionOfRelVelocityOn0 = constraint.normalTowardInside[1].dot(relVelocityOn0); } return true; } void CFSImpl::putContactPoints() { std::cout << "Contact Points\n"; for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair* linkPair = constrainedLinkPairs[i]; if(!linkPair->connection){ std::cout << " " << linkPair->link[0]->name() << " of " << linkPair->bodyData[0]->body->modelName(); std::cout << "<-->"; std::cout << " " << linkPair->link[1]->name() << " of " << linkPair->bodyData[1]->body->modelName(); ConstraintPointArray& constraintPoints = linkPair->constraintPoints; std::cout << "\n"; for(size_t j=0; j < constraintPoints.size(); ++j){ ConstraintPoint& contact = constraintPoints[j]; std::cout << " index " << contact.globalIndex; std::cout << " point: " << contact.point; std::cout << " normal: " << contact.normalTowardInside[1]; std::cout << " rel velocity: " << contact.relVelocityOn0; std::cout << " tangent: " << contact.frictionVector[0]; std::cout << "\n"; } } } std::cout << std::endl; } void CFSImpl::solveImpactConstraints() { if(CFS_DEBUG) std::cout << "Impacts !" << std::endl; } void CFSImpl::initMatrices() { const int n = globalNumConstraintVectors; const int m = globalNumFrictionVectors; const int dimLCP = usePivotingLCP ? (n + m + m) : (n + m); Mlcp.resize(dimLCP, dimLCP); b.resize(dimLCP); solution.resize(dimLCP); if(usePivotingLCP){ Mlcp.block(0, n + m, n, m).setZero(); Mlcp.block(n + m, 0, m, n).setZero(); Mlcp.block(n + m, n, m, m) = -MatrixX::Identity(m, m); Mlcp.block(n + m, n + m, m, m).setZero(); Mlcp.block(n, n + m, m, m).setIdentity(); b.tail(m).setZero(); } else { frictionIndexToContactIndex.resize(m); contactIndexToMu.resize(globalNumContactNormalVectors); mcpHi.resize(globalNumContactNormalVectors); } an0.resize(n); at0.resize(m); } void CFSImpl::setAccelCalcSkipInformation() { // clear skip check numbers for(size_t i=0; i < bodiesData.size(); ++i){ BodyData& bodyData = bodiesData[i]; if(bodyData.hasConstrainedLinks){ LinkDataArray& linksData = bodyData.linksData; for(size_t j=0; j < linksData.size(); ++j){ linksData[j].numberToCheckAccelCalcSkip = numeric_limits::max(); } } } // add the number of contact points to skip check numbers of the links from a contact target to the root int numLinkPairs = constrainedLinkPairs.size(); for(int i=0; i < numLinkPairs; ++i){ LinkPair* linkPair = constrainedLinkPairs[i]; int constraintIndex = linkPair->constraintPoints.front().globalIndex; for(int j=0; j < 2; ++j){ LinkDataArray& linksData = linkPair->bodyData[j]->linksData; int linkIndex = linkPair->link[j]->index; while(linkIndex >= 0){ LinkData& linkData = linksData[linkIndex]; if(linkData.numberToCheckAccelCalcSkip < constraintIndex){ break; } linkData.numberToCheckAccelCalcSkip = constraintIndex; linkIndex = linkData.parentIndex; } } } } void CFSImpl::setDefaultAccelerationVector() { // calculate accelerations with no constraint force for(size_t i=0; i < bodiesData.size(); ++i){ BodyData& bodyData = bodiesData[i]; if(bodyData.hasConstrainedLinks && ! bodyData.isStatic){ if(bodyData.forwardDynamicsMM){ bodyData.rootLinkPosRef = &(bodyData.body->rootLink()->p); Vector3 zeroForce = Vector3::Zero(); bodyData.forwardDynamicsMM->initializeAccelSolver(); bodyData.forwardDynamicsMM->solveUnknownAccels(zeroForce, zeroForce); calcAccelsMM(bodyData, numeric_limits::max()); } else { initABMForceElementsWithNoExtForce(bodyData); calcAccelsABM(bodyData, numeric_limits::max()); } } } // extract accelerations for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair& linkPair = *constrainedLinkPairs[i]; ConstraintPointArray& constraintPoints = linkPair.constraintPoints; for(size_t j=0; j < constraintPoints.size(); ++j){ ConstraintPoint& constraint = constraintPoints[j]; for(int k=0; k < 2; ++k){ if(linkPair.bodyData[k]->isStatic){ constraint.defaultAccel[k].setZero(); } else { Link* link = linkPair.link[k]; LinkData* linkData = linkPair.linkData[k]; constraint.defaultAccel[k] = linkData->dvo - constraint.point.cross(linkData->dw) + link->w.cross(link->vo + link->w.cross(constraint.point)); } } Vector3 relDefaultAccel(constraint.defaultAccel[1] - constraint.defaultAccel[0]); an0[constraint.globalIndex] = constraint.normalTowardInside[1].dot(relDefaultAccel); for(int k=0; k < constraint.numFrictionVectors; ++k){ at0[constraint.globalFrictionIndex + k] = constraint.frictionVector[k][1].dot(relDefaultAccel); } } } } void CFSImpl::setAccelerationMatrix() { const int n = globalNumConstraintVectors; const int m = globalNumFrictionVectors; Eigen::Block Knn = Mlcp.block(0, 0, n, n); Eigen::Block Ktn = Mlcp.block(0, n, n, m); Eigen::Block Knt = Mlcp.block(n, 0, m, n); Eigen::Block Ktt = Mlcp.block(n, n, m, m); for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair& linkPair = *constrainedLinkPairs[i]; int numConstraintsInPair = linkPair.constraintPoints.size(); for(int j=0; j < numConstraintsInPair; ++j){ ConstraintPoint& constraint = linkPair.constraintPoints[j]; int constraintIndex = constraint.globalIndex; // apply test normal force for(int k=0; k < 2; ++k){ BodyData& bodyData = *linkPair.bodyData[k]; if(!bodyData.isStatic){ bodyData.isTestForceBeingApplied = true; const Vector3& f = constraint.normalTowardInside[k]; if(bodyData.forwardDynamicsMM){ //! \todo This code does not work correctly when the links are in the same body. Fix it. Vector3 arm = constraint.point - *(bodyData.rootLinkPosRef); Vector3 tau = arm.cross(f); Vector3 tauext = constraint.point.cross(f); bodyData.forwardDynamicsMM->solveUnknownAccels(linkPair.link[k], f, tauext, f, tau); calcAccelsMM(bodyData, constraintIndex); } else { Vector3 tau = constraint.point.cross(f); calcABMForceElementsWithTestForce(bodyData, linkPair.link[k], f, tau); if(!linkPair.isSameBodyPair || (k > 0)){ calcAccelsABM(bodyData, constraintIndex); } } } } extractRelAccelsOfConstraintPoints(Knn, Knt, constraintIndex, constraintIndex); // apply test friction force for(int l=0; l < constraint.numFrictionVectors; ++l){ for(int k=0; k < 2; ++k){ BodyData& bodyData = *linkPair.bodyData[k]; if(!bodyData.isStatic){ const Vector3& f = constraint.frictionVector[l][k]; if(bodyData.forwardDynamicsMM){ //! \todo This code does not work correctly when the links are in the same body. Fix it. Vector3 arm = constraint.point - *(bodyData.rootLinkPosRef); Vector3 tau = arm.cross(f); Vector3 tauext = constraint.point.cross(f); bodyData.forwardDynamicsMM->solveUnknownAccels(linkPair.link[k], f, tauext, f, tau); calcAccelsMM(bodyData, constraintIndex); } else { Vector3 tau = constraint.point.cross(f); calcABMForceElementsWithTestForce(bodyData, linkPair.link[k], f, tau); if(!linkPair.isSameBodyPair || (k > 0)){ calcAccelsABM(bodyData, constraintIndex); } } } } extractRelAccelsOfConstraintPoints(Ktn, Ktt, constraint.globalFrictionIndex + l, constraintIndex); } linkPair.bodyData[0]->isTestForceBeingApplied = false; linkPair.bodyData[1]->isTestForceBeingApplied = false; } } if(ASSUME_SYMMETRIC_MATRIX){ copySymmetricElementsOfAccelerationMatrix(Knn, Ktn, Knt, Ktt); } } void CFSImpl::initABMForceElementsWithNoExtForce(BodyData& bodyData) { bodyData.dpf.setZero(); bodyData.dptau.setZero(); std::vector& linksData = bodyData.linksData; const LinkTraverse& traverse = bodyData.body->linkTraverse(); int n = traverse.numLinks(); for(int i = n-1; i >= 0; --i){ Link* link = traverse[i]; LinkData& data = linksData[i]; data.pf0 = link->pf; data.ptau0 = link->ptau; for(Link* child = link->child; child; child = child->sibling){ LinkData& childData = linksData[child->index]; data.pf0 += childData.pf0; data.ptau0 += childData.ptau0; if(child->jointType != Link::FIXED_JOINT ){ double uu_dd = childData.uu0 / child->dd; data.pf0 += uu_dd * child->hhv; data.ptau0 += uu_dd * child->hhw; } } if(i > 0){ if(link->jointType != Link::FIXED_JOINT){ data.uu0 = link->uu + link->u - (link->sv.dot(data.pf0) + link->sw.dot(data.ptau0)); data.uu = data.uu0; } } } } void CFSImpl::calcABMForceElementsWithTestForce (BodyData& bodyData, Link* linkToApplyForce, const Vector3& f, const Vector3& tau) { std::vector& linksData = bodyData.linksData; Vector3 dpf = -f; Vector3 dptau = -tau; Link* link = linkToApplyForce; while(link->parent){ if(link->jointType != Link::FIXED_JOINT){ LinkData& data = linksData[link->index]; double duu = -(link->sv.dot(dpf) + link->sw.dot(dptau)); data.uu += duu; double duudd = duu / link->dd; dpf += duudd * link->hhv; dptau += duudd * link->hhw; } link = link->parent; } bodyData.dpf += dpf; bodyData.dptau += dptau; } void CFSImpl::calcAccelsABM(BodyData& bodyData, int constraintIndex) { std::vector& linksData = bodyData.linksData; LinkData& rootData = linksData[0]; Link* rootLink = rootData.link; if(rootLink->jointType == Link::FREE_JOINT){ Eigen::Matrix M; M << rootLink->Ivv, rootLink->Iwv.transpose(), rootLink->Iwv, rootLink->Iww; Eigen::Matrix f; f << (rootData.pf0 + bodyData.dpf), (rootData.ptau0 + bodyData.dptau); f *= -1.0; Eigen::Matrix a(M.colPivHouseholderQr().solve(f)); rootData.dvo = a.head<3>(); rootData.dw = a.tail<3>(); } else { rootData.dw .setZero(); rootData.dvo.setZero(); } // reset bodyData.dpf .setZero(); bodyData.dptau.setZero(); int skipCheckNumber = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : (numeric_limits::max() - 1); int n = linksData.size(); for(int linkIndex = 1; linkIndex < n; ++linkIndex){ LinkData& linkData = linksData[linkIndex]; if(!SKIP_REDUNDANT_ACCEL_CALC || linkData.numberToCheckAccelCalcSkip <= skipCheckNumber){ Link* link = linkData.link; LinkData& parentData = linksData[linkData.parentIndex]; if(link->jointType != Link::FIXED_JOINT){ linkData.ddq = (linkData.uu - (link->hhv.dot(parentData.dvo) + link->hhw.dot(parentData.dw))) / link->dd; linkData.dvo = parentData.dvo + link->cv + link->sv * linkData.ddq; linkData.dw = parentData.dw + link->cw + link->sw * linkData.ddq; }else{ linkData.ddq = 0.0; linkData.dvo = parentData.dvo; linkData.dw = parentData.dw; } // reset linkData.uu = linkData.uu0; } } } void CFSImpl::calcAccelsMM(BodyData& bodyData, int constraintIndex) { std::vector& linksData = bodyData.linksData; LinkData& rootData = linksData[0]; Link* rootLink = rootData.link; rootData.dvo = rootLink->dvo; rootData.dw = rootLink->dw; int skipCheckNumber = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : (numeric_limits::max() - 1); int n = linksData.size(); for(int linkIndex = 1; linkIndex < n; ++linkIndex){ LinkData& linkData = linksData[linkIndex]; if(!SKIP_REDUNDANT_ACCEL_CALC || linkData.numberToCheckAccelCalcSkip <= skipCheckNumber){ Link* link = linkData.link; LinkData& parentData = linksData[linkData.parentIndex]; if(link->jointType != Link::FIXED_JOINT){ linkData.dvo = parentData.dvo + link->cv + link->ddq * link->sv; linkData.dw = parentData.dw + link->cw + link->ddq * link->sw; }else{ linkData.dvo = parentData.dvo; linkData.dw = parentData.dw; } } } } void CFSImpl::extractRelAccelsOfConstraintPoints (Eigen::Block& Kxn, Eigen::Block& Kxt, int testForceIndex, int constraintIndex) { int maxConstraintIndexToExtract = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : globalNumConstraintVectors; for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair& linkPair = *constrainedLinkPairs[i]; BodyData& bodyData0 = *linkPair.bodyData[0]; BodyData& bodyData1 = *linkPair.bodyData[1]; if(bodyData0.isTestForceBeingApplied){ if(bodyData1.isTestForceBeingApplied){ extractRelAccelsFromLinkPairCase1(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract); } else { extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 0, 1, testForceIndex, maxConstraintIndexToExtract); } } else { if(bodyData1.isTestForceBeingApplied){ extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 1, 0, testForceIndex, maxConstraintIndexToExtract); } else { extractRelAccelsFromLinkPairCase3(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract); } } } } void CFSImpl::extractRelAccelsFromLinkPairCase1 (Eigen::Block& Kxn, Eigen::Block& Kxt, LinkPair& linkPair, int testForceIndex, int maxConstraintIndexToExtract) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; for(size_t i=0; i < constraintPoints.size(); ++i){ ConstraintPoint& constraint = constraintPoints[i]; int constraintIndex = constraint.globalIndex; if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){ break; } Link* link0 = linkPair.link[0]; Link* link1 = linkPair.link[1]; LinkData* linkData0 = linkPair.linkData[0]; LinkData* linkData1 = linkPair.linkData[1]; //! \todo Can the follwoing equations be simplified ? Vector3 dv0 = linkData0->dvo - constraint.point.cross(linkData0->dw) + link0->w.cross(link0->vo + link0->w.cross(constraint.point)); Vector3 dv1 = linkData1->dvo - constraint.point.cross(linkData1->dw) + link1->w.cross(link1->vo + link1->w.cross(constraint.point)); Vector3 relAccel = dv1 - dv0; Kxn(constraintIndex, testForceIndex) = constraint.normalTowardInside[1].dot(relAccel) - an0(constraintIndex); for(int j=0; j < constraint.numFrictionVectors; ++j){ const int index = constraint.globalFrictionIndex + j; Kxt(index, testForceIndex) = constraint.frictionVector[j][1].dot(relAccel) - at0(index); } } } void CFSImpl::extractRelAccelsFromLinkPairCase2 (Eigen::Block& Kxn, Eigen::Block& Kxt, LinkPair& linkPair, int iTestForce, int iDefault, int testForceIndex, int maxConstraintIndexToExtract) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; for(size_t i=0; i < constraintPoints.size(); ++i){ ConstraintPoint& constraint = constraintPoints[i]; int constraintIndex = constraint.globalIndex; if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){ break; } Link* link = linkPair.link[iTestForce]; LinkData* linkData = linkPair.linkData[iTestForce]; Vector3 dv(linkData->dvo - constraint.point.cross(linkData->dw) + link->w.cross(link->vo + link->w.cross(constraint.point))); if(CFS_DEBUG_VERBOSE_2){ std::cout << "dv " << constraintIndex << " = " << dv << "\n"; } Vector3 relAccel = constraint.defaultAccel[iDefault] - dv; Kxn(constraintIndex, testForceIndex) = constraint.normalTowardInside[iDefault].dot(relAccel) - an0(constraintIndex); for(int j=0; j < constraint.numFrictionVectors; ++j){ const int index = constraint.globalFrictionIndex + j; Kxt(index, testForceIndex) = constraint.frictionVector[j][iDefault].dot(relAccel) - at0(index); } } } void CFSImpl::extractRelAccelsFromLinkPairCase3 (Eigen::Block& Kxn, Eigen::Block& Kxt, LinkPair& linkPair, int testForceIndex, int maxConstraintIndexToExtract) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; for(size_t i=0; i < constraintPoints.size(); ++i){ ConstraintPoint& constraint = constraintPoints[i]; int constraintIndex = constraint.globalIndex; if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){ break; } Kxn(constraintIndex, testForceIndex) = 0.0; for(int j=0; j < constraint.numFrictionVectors; ++j){ Kxt(constraint.globalFrictionIndex + j, testForceIndex) = 0.0; } } } void CFSImpl::copySymmetricElementsOfAccelerationMatrix (Eigen::Block& Knn, Eigen::Block& Ktn, Eigen::Block& Knt, Eigen::Block& Ktt) { for(size_t linkPairIndex=0; linkPairIndex < constrainedLinkPairs.size(); ++linkPairIndex){ ConstraintPointArray& constraintPoints = constrainedLinkPairs[linkPairIndex]->constraintPoints; for(size_t localConstraintIndex = 0; localConstraintIndex < constraintPoints.size(); ++localConstraintIndex){ ConstraintPoint& constraint = constraintPoints[localConstraintIndex]; int constraintIndex = constraint.globalIndex; int nextConstraintIndex = constraintIndex + 1; for(int i = nextConstraintIndex; i < globalNumConstraintVectors; ++i){ Knn(i, constraintIndex) = Knn(constraintIndex, i); } int frictionTopOfNextConstraint = constraint.globalFrictionIndex + constraint.numFrictionVectors; for(int i = frictionTopOfNextConstraint; i < globalNumFrictionVectors; ++i){ Knt(i, constraintIndex) = Ktn(constraintIndex, i); } for(int localFrictionIndex=0; localFrictionIndex < constraint.numFrictionVectors; ++localFrictionIndex){ int frictionIndex = constraint.globalFrictionIndex + localFrictionIndex; for(int i = nextConstraintIndex; i < globalNumConstraintVectors; ++i){ Ktn(i, frictionIndex) = Knt(frictionIndex, i); } for(int i = frictionTopOfNextConstraint; i < globalNumFrictionVectors; ++i){ Ktt(i, frictionIndex) = Ktt(frictionIndex, i); } } } } } void CFSImpl::clearSingularPointConstraintsOfClosedLoopConnections() { for(int i = 0; i < Mlcp.rows(); ++i){ if(Mlcp(i, i) < 1.0e-4){ for(int j=0; j < Mlcp.rows(); ++j){ Mlcp(j, i) = 0.0; } Mlcp(i, i) = numeric_limits::max(); } } } void CFSImpl::setConstantVectorAndMuBlock() { double dtinv = 1.0 / world.timeStep(); const int block2 = globalNumConstraintVectors; const int block3 = globalNumConstraintVectors + globalNumFrictionVectors; for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair& linkPair = *constrainedLinkPairs[i]; int numConstraintsInPair = linkPair.constraintPoints.size(); for(int j=0; j < numConstraintsInPair; ++j){ ConstraintPoint& constraint = linkPair.constraintPoints[j]; int globalIndex = constraint.globalIndex; // set constant vector of LCP // constraints for normal acceleration if(linkPair.connection){ // connection constraint const double& error = constraint.depth; double v; if(error >= 0){ v = 0.1 * (-1.0 + exp(-error * 20.0)); } else { v = 0.1 * ( 1.0 - exp( error * 20.0)); } b(globalIndex) = an0(globalIndex) + (constraint.normalProjectionOfRelVelocityOn0 + v) * dtinv; } else { // contact constraint if(ALLOW_SUBTLE_PENETRATION_FOR_STABILITY){ double extraNegativeVel; double newDepth = ALLOWED_PENETRATION_DEPTH - constraint.depth; extraNegativeVel = negativeVelocityRatioForPenetration * newDepth; b(globalIndex) = an0(globalIndex) + (constraint.normalProjectionOfRelVelocityOn0 + extraNegativeVel) * dtinv; } else { b(globalIndex) = an0(globalIndex) + constraint.normalProjectionOfRelVelocityOn0 * dtinv; } contactIndexToMu[globalIndex] = constraint.mu; int globalFrictionIndex = constraint.globalFrictionIndex; for(int k=0; k < constraint.numFrictionVectors; ++k){ // constraints for tangent acceleration double tangentProjectionOfRelVelocity = constraint.frictionVector[k][1].dot(constraint.relVelocityOn0); b(block2 + globalFrictionIndex) = at0(globalFrictionIndex); if( !IGNORE_CURRENT_VELOCITY_IN_STATIC_FRICTION || constraint.numFrictionVectors == 1){ b(block2 + globalFrictionIndex) += tangentProjectionOfRelVelocity * dtinv; } if(usePivotingLCP){ // set mu (coefficients of friction) Mlcp(block3 + globalFrictionIndex, globalIndex) = constraint.mu; } else { // for iterative solver frictionIndexToContactIndex[globalFrictionIndex] = globalIndex; } ++globalFrictionIndex; } } } } } void CFSImpl::addConstraintForceToLinks() { int n = constrainedLinkPairs.size(); for(int i=0; i < n; ++i){ LinkPair* linkPair = constrainedLinkPairs[i]; for(int j=0; j < 2; ++j){ // if(!linkPair->link[j]->isRoot() || linkPair->link[j]->jointType != Link::FIXED_JOINT){ addConstraintForceToLink(linkPair, j); // } } } } void CFSImpl::addConstraintForceToLink(LinkPair* linkPair, int ipair) { Vector3 f_total = Vector3::Zero(); Vector3 tau_total = Vector3::Zero(); ConstraintPointArray& constraintPoints = linkPair->constraintPoints; int numConstraintPoints = constraintPoints.size(); Link* link = linkPair->link[ipair]; for(int i=0; i < numConstraintPoints; ++i){ ConstraintPoint& constraint = constraintPoints[i]; int globalIndex = constraint.globalIndex; Vector3 f = solution(globalIndex) * constraint.normalTowardInside[ipair]; for(int j=0; j < constraint.numFrictionVectors; ++j){ f += solution(globalNumConstraintVectors + constraint.globalFrictionIndex + j) * constraint.frictionVector[j][ipair]; } f_total += f; tau_total += constraint.point.cross(f); if(isConstraintForceOutputMode){ Link::ConstraintForceArray& constraintForces = link->constraintForces; constraintForces.resize(constraintForces.size() + 1); Link::ConstraintForce& cforce = constraintForces.back(); cforce.point = constraint.point; cforce.force = f; } } link->fext += f_total; link->tauext += tau_total; if(CFS_DEBUG){ std::cout << "Constraint force to " << link->name() << ": f = " << f_total << ", tau = " << tau_total << std::endl; } } void CFSImpl::solveMCPByProjectedGaussSeidel(const MatrixX& M, const VectorX& b, VectorX& x) { static const int loopBlockSize = DEFAULT_NUM_GAUSS_SEIDEL_ITERATION_BLOCK; if(numGaussSeidelInitialIteration > 0){ solveMCPByProjectedGaussSeidelInitial(M, b, x, numGaussSeidelInitialIteration); } int numBlockLoops = maxNumGaussSeidelIteration / loopBlockSize; if(numBlockLoops==0) numBlockLoops = 1; if(CFS_MCP_DEBUG) cout << "Iteration "; double error = 0.0; int i = 0; while(i < numBlockLoops){ i++; solveMCPByProjectedGaussSeidelMain(M, b, x, loopBlockSize - 1); error = solveMCPByProjectedGaussSeidelErrorCheck(M, b, x); if(error < gaussSeidelMaxRelError){ if(CFS_MCP_DEBUG_SHOW_ITERATION_STOP) cout << "stopped at " << i * loopBlockSize << endl; break; } } if(CFS_MCP_DEBUG){ int n = loopBlockSize * i; numGaussSeidelTotalLoops += n; numGaussSeidelTotalCalls++; cout << n; cout << ", avarage = " << (numGaussSeidelTotalLoops / numGaussSeidelTotalCalls); cout << ", error = " << error; cout << endl; } } void CFSImpl::solveMCPByProjectedGaussSeidelInitial (const MatrixX& M, const VectorX& b, VectorX& x, const int numIteration) { const int size = globalNumConstraintVectors + globalNumFrictionVectors; const double rstep = 1.0 / (numIteration * size); double r = 0.0; for(int i=0; i < numIteration; ++i){ for(int j=0; j < globalNumContactNormalVectors; ++j){ double xx; if(M(j,j)==numeric_limits::max()) xx=0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } xx = (-b(j) - sum) / M(j, j); } if(xx < 0.0){ x(j) = 0.0; } else { x(j) = r * xx; } r += rstep; mcpHi[j] = contactIndexToMu[j] * x(j); } for(int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){ if(M(j,j)==numeric_limits::max()) x(j) = 0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } x(j) = r * (-b(j) - sum) / M(j, j); } r += rstep; } if(ENABLE_TRUE_FRICTION_CONE){ int contactIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){ double fx0; if(M(j,j)==numeric_limits::max()) fx0 = 0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } fx0 = (-b(j) - sum) / M(j, j); } double& fx = x(j); ++j; double fy0; if(M(j,j)==numeric_limits::max()) fy0 = 0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } fy0 = (-b(j) - sum) / M(j, j); } double& fy = x(j); const double fmax = mcpHi[contactIndex]; const double fmax2 = fmax * fmax; const double fmag2 = fx0 * fx0 + fy0 * fy0; if(fmag2 > fmax2){ const double s = r * fmax / sqrt(fmag2); fx = s * fx0; fy = s * fy0; } else { fx = r * fx0; fy = r * fy0; } r += (rstep + rstep); } } else { int frictionIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){ double xx; if(M(j,j)==numeric_limits::max()) xx = 0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } xx = (-b(j) - sum) / M(j, j); } const int contactIndex = frictionIndexToContactIndex[frictionIndex]; const double fmax = mcpHi[contactIndex]; const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0); if(xx < fmin){ x(j) = fmin; } else if(xx > fmax){ x(j) = fmax; } else { x(j) = xx; } x(j) *= r; r += rstep; } } } } void CFSImpl::solveMCPByProjectedGaussSeidelMain (const MatrixX& M, const VectorX& b, VectorX& x, const int numIteration) { const int size = globalNumConstraintVectors + globalNumFrictionVectors; for(int i=0; i < numIteration; ++i){ for(int j=0; j < globalNumContactNormalVectors; ++j){ double xx; if(M(j,j)==numeric_limits::max()) xx=0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } xx = (-b(j) - sum) / M(j, j); } if(xx < 0.0){ x(j) = 0.0; } else { x(j) = xx; } mcpHi[j] = contactIndexToMu[j] * x(j); } for(int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){ if(M(j,j)==numeric_limits::max()) x(j)=0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } x(j) = (-b(j) - sum) / M(j, j); } } if(ENABLE_TRUE_FRICTION_CONE){ int contactIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){ double fx0; if(M(j,j)==numeric_limits::max()) fx0=0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } fx0 = (-b(j) - sum) / M(j, j); } double& fx = x(j); ++j; double fy0; if(M(j,j)==numeric_limits::max()) fy0=0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } fy0 = (-b(j) - sum) / M(j, j); } double& fy = x(j); const double fmax = mcpHi[contactIndex]; const double fmax2 = fmax * fmax; const double fmag2 = fx0 * fx0 + fy0 * fy0; if(fmag2 > fmax2){ const double s = fmax / sqrt(fmag2); fx = s * fx0; fy = s * fy0; } else { fx = fx0; fy = fy0; } } } else { int frictionIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){ double xx; if(M(j,j)==numeric_limits::max()) xx=0.0; else{ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } xx = (-b(j) - sum) / M(j, j); } const int contactIndex = frictionIndexToContactIndex[frictionIndex]; const double fmax = mcpHi[contactIndex]; const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0); if(xx < fmin){ x(j) = fmin; } else if(xx > fmax){ x(j) = fmax; } else { x(j) = xx; } } } } } double CFSImpl::solveMCPByProjectedGaussSeidelErrorCheck (const MatrixX& M, const VectorX& b, VectorX& x) { const int size = globalNumConstraintVectors + globalNumFrictionVectors; double error = 0.0; for(int j=0; j < globalNumConstraintVectors; ++j){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } double xx = (-b(j) - sum) / M(j, j); if(j < globalNumContactNormalVectors){ if(xx < 0.0){ xx = 0.0; } mcpHi[j] = contactIndexToMu[j] * xx; } double d = fabs(xx - x(j)); if(xx > numeric_limits::epsilon()){ d /= xx; } if(d > error){ error = d; } x(j) = xx; } if(ENABLE_TRUE_FRICTION_CONE){ int contactIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } double fx0 = (-b(j) - sum) / M(j, j); double& fx = x(j); ++j; sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } double fy0 = (-b(j) - sum) / M(j, j); double& fy = x(j); const double fmax = mcpHi[contactIndex]; const double fmax2 = fmax * fmax; const double fmag2 = fx0 * fx0 + fy0 * fy0; if(fmag2 > fmax2){ const double s = fmax / sqrt(fmag2); fx0 *= s; fy0 *= s; } double d = fabs(fx0 - fx); const double afx0 = fabs(fx0); if(afx0 > numeric_limits::epsilon()){ d /= afx0; } if(d > error){ error = d; } d = fabs(fy0 - fy); const double afy0 = fabs(fy0); if(afy0 > numeric_limits::epsilon()){ d /= afy0; } if(d > error){ error = d; } fx = fx0; fy = fy0; } } else { int frictionIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } double xx = (-b(j) - sum) / M(j, j); const int contactIndex = frictionIndexToContactIndex[frictionIndex]; const double fmax = mcpHi[contactIndex]; const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0); if(xx < fmin){ xx = fmin; } else if(xx > fmax){ xx = fmax; } double d = fabs(xx - x(j)); if(xx > numeric_limits::epsilon()){ d /= xx; } if(d > error){ error = d; } x(j) = xx; } } return error; } void CFSImpl::checkLCPResult(MatrixX& M, VectorX& b, VectorX& x) { std::cout << "check LCP result\n"; std::cout << "-------------------------------\n"; VectorX z = M * x + b; int n = x.size(); for(int i=0; i < n; ++i){ std::cout << "(" << x(i) << ", " << z(i) << ")"; if(x(i) < 0.0 || z(i) < 0.0 || x(i) * z(i) != 0.0){ std::cout << " - X"; } std::cout << "\n"; if(i == globalNumConstraintVectors){ std::cout << "-------------------------------\n"; } else if(i == globalNumConstraintVectors + globalNumFrictionVectors){ std::cout << "-------------------------------\n"; } } std::cout << "-------------------------------\n"; std::cout << std::endl; } void CFSImpl::checkMCPResult(MatrixX& M, VectorX& b, VectorX& x) { std::cout << "check MCP result\n"; std::cout << "-------------------------------\n"; VectorX z = M * x + b; for(int i=0; i < globalNumConstraintVectors; ++i){ std::cout << "(" << x(i) << ", " << z(i) << ")"; if(x(i) < 0.0 || z(i) < -1.0e-6){ std::cout << " - X"; } else if(x(i) > 0.0 && fabs(z(i)) > 1.0e-6){ std::cout << " - X"; } else if(z(i) > 1.0e-6 && fabs(x(i)) > 1.0e-6){ std::cout << " - X"; } std::cout << "\n"; } std::cout << "-------------------------------\n"; int j = 0; for(int i=globalNumConstraintVectors; i < globalNumConstraintVectors + globalNumFrictionVectors; ++i, ++j){ std::cout << "(" << x(i) << ", " << z(i) << ")"; int contactIndex = frictionIndexToContactIndex[j]; double hi = contactIndexToMu[contactIndex] * x(contactIndex); std::cout << " hi = " << hi; if(x(i) < 0.0 || x(i) > hi){ std::cout << " - X"; } else if(x(i) == hi && z(i) > -1.0e-6){ std::cout << " - X"; } else if(x(i) < hi && x(i) > 0.0 && fabs(z(i)) > 1.0e-6){ std::cout << " - X"; } std::cout << "\n"; } std::cout << "-------------------------------\n"; std::cout << std::endl; } #ifdef USE_PIVOTING_LCP bool CFSImpl::callPathLCPSolver(MatrixX& Mlcp, VectorX& b, VectorX& solution) { int size = solution.size(); int square = size * size; std::vector lb(size + 1, 0.0); std::vector ub(size + 1, 1.0e20); int m_nnz = 0; std::vector m_i(square + 1); std::vector m_j(square + 1); std::vector m_ij(square + 1); for(int i=0; i < size; ++i){ solution(i) = 0.0; } for(int j=0; j < size; ++j){ for(int i=0; i < size; ++i){ double v = Mlcp(i, j); if(v != 0.0){ m_i[m_nnz] = i+1; m_j[m_nnz] = j+1; m_ij[m_nnz] = v; ++m_nnz; } } } MCP_Termination status; SimpleLCP(size, m_nnz, &m_i[0], &m_j[0], &m_ij[0], &b(0), &lb[0], &ub[0], &status, &solution(0)); return (status == MCP_Solved); } #endif ConstraintForceSolver::ConstraintForceSolver(WorldBase& world) { impl = new CFSImpl(world); } ConstraintForceSolver::~ConstraintForceSolver() { delete impl; } bool ConstraintForceSolver::addCollisionCheckLinkPair (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double epsilon) { return impl->addCollisionCheckLinkPair(bodyIndex1, link1, bodyIndex2, link2, muStatic, muDynamic, culling_thresh, epsilon); } void ConstraintForceSolver::clearCollisionCheckLinkPairs() { impl->world.clearCollisionPairs(); impl->collisionCheckLinkPairs.clear(); } void ConstraintForceSolver::setGaussSeidelParameters(int maxNumIteration, int numInitialIteration, double maxRelError) { impl->maxNumGaussSeidelIteration = maxNumIteration; impl->numGaussSeidelInitialIteration = numInitialIteration; impl->gaussSeidelMaxRelError = maxRelError; } void ConstraintForceSolver::enableConstraintForceOutput(bool on) { impl->isConstraintForceOutputMode = on; } void ConstraintForceSolver::setNegativeVelocityRatioForPenetration(double ratio) { impl->negativeVelocityRatioForPenetration = ratio; } void ConstraintForceSolver::initialize(void) { impl->initialize(); } void ConstraintForceSolver::solve() { impl->solve(); } void ConstraintForceSolver::clearExternalForces() { impl->clearExternalForces(); } choreonoid-1.1.0+dfsg/src/Body/ConstraintForceSolver.h000066400000000000000000000021221207742442300227160ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_CONSTRAINT_FORCE_SOLVER_H_INCLUDED #define CNOID_BODY_CONSTRAINT_FORCE_SOLVER_H_INCLUDED #include "exportdecl.h" namespace cnoid { class Link; class CFSImpl; class WorldBase; class CNOID_EXPORT ConstraintForceSolver { CFSImpl* impl; public: ConstraintForceSolver(WorldBase& world); ~ConstraintForceSolver(); bool addCollisionCheckLinkPair (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double epsilon); void clearCollisionCheckLinkPairs(); void setGaussSeidelParameters(int maxNumIteration, int numInitialIteration, double maxRelError); bool enableJointRangeStopper(bool isEnabled); bool enableVelocityOverwriting(bool isEnabled); void enableConstraintForceOutput(bool on); void setNegativeVelocityRatioForPenetration(double ratio); void initialize(void); void solve(); void clearExternalForces(); }; }; #endif choreonoid-1.1.0+dfsg/src/Body/ConstraintForceSolver.penalty.cpp000066400000000000000000002170011207742442300247300ustar00rootroot00000000000000/* * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc. * All rights reserved. This program is made available under the terms of the * Eclipse Public License v1.0 which accompanies this distribution, and is * available at http://www.eclipse.org/legal/epl-v10.html * Contributors: * National Institute of Advanced Industrial Science and Technology (AIST) */ /** \file \brief Implementation of ConstraintForceSolver class \author Shin'ichiro Nakaoka */ #ifdef __WIN32__ #define NOMINMAX #endif #include "World.h" #include "Body.h" #include "Link.h" #include "LinkTraverse.h" #include "ForwardDynamicsCBM.h" #include "ConstraintForceSolver.h" #include #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace tvmet; using namespace boost::numeric::ublas; using namespace cnoid; using namespace OpenHRP; // Is LCP solved by Iterative or Pivoting method ? // #define USE_PIVOTING_LCP #ifdef USE_PIVOTING_LCP #include "SimpleLCP_Path.h" static const bool usePivotingLCP = true; #else static const bool usePivotingLCP = false; #endif // settings static const double VEL_THRESH_OF_DYNAMIC_FRICTION = 1.0e-4; static const bool ENABLE_STATIC_FRICTION = true; static const bool ONLY_STATIC_FRICTION_FORMULATION = (true && ENABLE_STATIC_FRICTION); static const bool STATIC_FRICTION_BY_TWO_CONSTRAINTS = true; static const bool IGNORE_CURRENT_VELOCITY_IN_STATIC_FRICTION = false; static const bool ENABLE_TRUE_FRICTION_CONE = (true && ONLY_STATIC_FRICTION_FORMULATION && STATIC_FRICTION_BY_TWO_CONSTRAINTS); static const bool SKIP_REDUNDANT_ACCEL_CALC = true; static const bool ASSUME_SYMMETRIC_MATRIX = false; static const int DEFAULT_MAX_NUM_GAUSS_SEIDEL_ITERATION = 500; static const int DEFAULT_NUM_GAUSS_SEIDEL_ITERATION_BLOCK = 10; static const int DEFAULT_NUM_GAUSS_SEIDEL_INITIAL_ITERATION = 0; static const double DEFAULT_GAUSS_SEIDEL_MAX_REL_ERROR = 1.0e-3; static const bool USE_PREVIOUS_LCP_SOLUTION = true; static const bool ALLOW_SUBTLE_PENETRATION_FOR_STABILITY = true; // normal setting static const double ALLOWED_PENETRATION_DEPTH = 0.0001; //static const double PENETRATION_A = 500.0; //static const double PENETRATION_B = 80.0; static const double NEGATIVE_VELOCITY_RATIO_FOR_PENETRTION = 10.0; // test for mobile robots with wheels //static const double ALLOWED_PENETRATION_DEPTH = 0.005; //static const double PENETRATION_A = 500.0; //static const double PENETRATION_B = 80.0; //static const double NEGATIVE_VELOCITY_RATIO_FOR_PENETRTION = 10.0; //static const bool ENABLE_CONTACT_POINT_THINNING = false; // experimental options static const bool PROPORTIONAL_DYNAMIC_FRICTION = false; static const bool ENABLE_RANDOM_STATIC_FRICTION_BASE = false; // debug options static const bool CFS_DEBUG = false; static const bool CFS_DEBUG_VERBOSE = false; static const bool CFS_DEBUG_VERBOSE_2 = false; static const bool CFS_DEBUG_VERBOSE_3 = false; static const bool CFS_DEBUG_LCPCHECK = false; static const bool CFS_MCP_DEBUG = false; static const bool CFS_MCP_DEBUG_SHOW_ITERATION_STOP = false; static const bool CFS_PUT_NUM_CONTACT_POINTS = false; namespace cnoid { class CFSImpl { public: CFSImpl(WorldBase& world); bool addCollisionCheckLinkPair (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double epsilon); void initialize(void); void solve(CollisionSequence& corbaCollisionSequence); inline void clearExternalForces(); #undef PI #ifdef __WIN32__ // Visual C++ bug static const double PI; static const double PI_2; #elif defined(__APPLE__) #define PI M_PI #define PI_2 M_PI/2 #else static const double PI = 3.14159265358979323846; static const double PI_2 = 1.57079632679489661923; #endif WorldBase& world; bool isConstraintForceOutputMode; bool useBuiltinCollisionDetector; struct ConstraintPoint { int globalIndex; Vector3 point; Vector3 normalTowardInside[2]; Vector3 defaultAccel[2]; double normalProjectionOfRelVelocityOn0; double depth; // position error in the case of a connection point double mu; Vector3 relVelocityOn0; int globalFrictionIndex; int numFrictionVectors; Vector3 frictionVector[4][2]; }; typedef std::vector ConstraintPointArray; struct LinkData { Vector3 dvo; Vector3 dw; Vector3 pf0; Vector3 ptau0; double uu; double uu0; double ddq; int numberToCheckAccelCalcSkip; int parentIndex; Link* link; }; typedef std::vector LinkDataArray; struct BodyData { BodyPtr body; bool isStatic; bool hasConstrainedLinks; bool isTestForceBeingApplied; LinkDataArray linksData; Vector3 dpf; Vector3 dptau; /// only used by the ForwardDynamicsMM mode Vector3* rootLinkPosRef; /** If the body includes high-gain mode joints, the ForwardDynamisMM object of the body is set to this pointer. The pointer is null when all the joints are torque mode and the forward dynamics is calculated by ABM. */ ForwardDynamicsMMPtr forwardDynamicsMM; }; std::vector bodiesData; class LinkPair : public ColdetModelPair { public: int index; bool isSameBodyPair; int bodyIndex[2]; BodyData* bodyData[2]; Link* link[2]; LinkData* linkData[2]; ConstraintPointArray constraintPoints; double muStatic; double muDynamic; double culling_thresh; double epsilon; Body::LinkConnection* connection; }; typedef intrusive_ptr LinkPairPtr; typedef std::vector LinkPairArray; LinkPairArray collisionCheckLinkPairs; LinkPairArray connectedLinkPairs; std::vector constrainedLinkPairs; /** globalNumConstraintVectors = globalNumContactNormalVectors + globalNumConnectionVectors */ int globalNumConstraintVectors; int globalNumContactNormalVectors; int globalNumConnectionVectors; int globalNumFrictionVectors; int prevGlobalNumConstraintVectors; int prevGlobalNumFrictionVectors; bool areThereImpacts; int numUnconverged; // Mlcp * solution + b _|_ solution typedef boost::numeric::ublas::matrix rmdmatrix; rmdmatrix Mlcp; // constant acceleration term when no external force is applied dvector an0; dvector at0; // constant vector of LCP dvector b; // contact force solution: normal forces at contact points dvector solution; // random number generator variate_generator > randomAngle; // for special version of gauss sidel iterative solver std::vector frictionIndexToContactIndex; dvector contactIndexToMu; dvector mcpHi; int maxNumGaussSeidelIteration; int numGaussSeidelInitialIteration; double gaussSeidelMaxRelError; int numGaussSeidelTotalLoops; int numGaussSeidelTotalCalls; void setConstraintPoints(CollisionSequence& collisions); void setContactConstraintPoints(LinkPair& linkPair, CollisionPointSequence& collisionPoints); void setFrictionVectors(ConstraintPoint& constraintPoint); bool setConnectionConstraintPoints(LinkPair& linkPair); void putContactPoints(); void solveImpactConstraints(); void initMatrices(); void setAccelCalcSkipInformation(); void setDefaultAccelerationVector(); void setAccelerationMatrix(); void initABMForceElementsWithNoExtForce(BodyData& bodyData); void calcABMForceElementsWithTestForce(BodyData& bodyData, Link* linkToApplyForce, const Vector3& f, const Vector3& tau); void calcAccelsABM(BodyData& bodyData, int constraintIndex); void calcAccelsMM(BodyData& bodyData, int constraintIndex); void extractRelAccelsOfConstraintPoints (matrix_range& Kxn, matrix_range& Kxt, int testForceIndex, int constraintIndex); void extractRelAccelsFromLinkPairCase1 (matrix_range& Kxn, matrix_range& Kxt, LinkPair& linkPair, int testForceIndex, int constraintIndex); void extractRelAccelsFromLinkPairCase2 (matrix_range& Kxn, matrix_range& Kxt, LinkPair& linkPair, int iTestForce, int iDefault, int testForceIndex, int constraintIndex); void extractRelAccelsFromLinkPairCase3 (matrix_range& Kxn, matrix_range& Kxt, LinkPair& linkPair, int testForceIndex, int constraintIndex); void copySymmetricElementsOfAccelerationMatrix (matrix_range& Knn, matrix_range& Ktn, matrix_range& Knt, matrix_range& Ktt); void clearSingularPointConstraintsOfClosedLoopConnections(); void setConstantVectorAndMuBlock(); void addConstraintForceToLinks(); void addConstraintForceToLink(LinkPair* linkPair, int ipair); void solveMCPByProjectedGaussSeidel (const rmdmatrix& M, const dvector& b, dvector& x); void solveMCPByProjectedGaussSeidelInitial (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration); void solveMCPByProjectedGaussSeidelMain (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration); double solveMCPByProjectedGaussSeidelErrorCheck (const rmdmatrix& M, const dvector& b, dvector& x); void checkLCPResult(rmdmatrix& M, dvector& b, dvector& x); void checkMCPResult(rmdmatrix& M, dvector& b, dvector& x); #ifdef USE_PIVOTING_LCP bool callPathLCPSolver(rmdmatrix& Mlcp, dvector& b, dvector& solution); // for PATH solver std::vector lb; std::vector ub; std::vector m_i; std::vector m_j; std::vector m_ij; #endif }; #ifdef __WIN32__ const double CFSImpl::PI = 3.14159265358979323846; const double CFSImpl::PI_2 = 1.57079632679489661923; #endif }; template static void putMatrix(TMatrix& M, const char *name) { if(M.size2() == 1){ std::cout << "Vector " << name << M << std::endl; } else { std::cout << "Matrix " << name << ": \n"; for(size_t i=0; i < M.size1(); i++){ for(size_t j=0; j < M.size2(); j++){ std::cout << boost::format(" %6.3f ") % M(i, j); } std::cout << std::endl; } } } template static void putVector(TVector& M, const char *name) { std::cout << "Vector " << name << M << std::endl; } template static inline void debugPutMatrix(TMatrix& M, const char *name) { if(CFS_DEBUG_VERBOSE) putMatrix(M, name); } template static inline void debugPutVector(TVector& M, const char *name) { if(CFS_DEBUG_VERBOSE) putVector(M, name); } CFSImpl::CFSImpl(WorldBase& world) : world(world), randomAngle(mt19937(), uniform_real<>(0.0, 2.0 * PI)) { maxNumGaussSeidelIteration = DEFAULT_MAX_NUM_GAUSS_SEIDEL_ITERATION; numGaussSeidelInitialIteration = DEFAULT_NUM_GAUSS_SEIDEL_INITIAL_ITERATION; gaussSeidelMaxRelError = DEFAULT_GAUSS_SEIDEL_MAX_REL_ERROR; isConstraintForceOutputMode = false; useBuiltinCollisionDetector = false; } bool CFSImpl::addCollisionCheckLinkPair (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double epsilon) { int index; int isRegistered; tie(index, isRegistered) = world.getIndexOfLinkPairs(link1, link2); if(index >= 0){ int n = collisionCheckLinkPairs.size(); if(index >= n){ collisionCheckLinkPairs.resize(index+1); } LinkPairPtr& linkPair = collisionCheckLinkPairs[index]; if(!linkPair){ linkPair = new LinkPair(); } linkPair->set(link1->coldetModel, link2->coldetModel); linkPair->isSameBodyPair = (bodyIndex1 == bodyIndex2); linkPair->bodyIndex[0] = bodyIndex1; linkPair->link[0] = link1; linkPair->bodyIndex[1] = bodyIndex2; linkPair->link[1] = link2; linkPair->index = index; linkPair->muStatic = muStatic; linkPair->muDynamic = muDynamic; linkPair->culling_thresh = culling_thresh; linkPair->epsilon = epsilon; linkPair->connection = 0; } return (index >= 0 && !isRegistered); } void CFSImpl::initialize(void) { if(CFS_MCP_DEBUG){ numGaussSeidelTotalCalls = 0; numGaussSeidelTotalLoops = 0; } int numBodies = world.numBodies(); bodiesData.resize(numBodies); connectedLinkPairs.clear(); for(int bodyIndex=0; bodyIndex < numBodies; ++bodyIndex){ BodyPtr body = world.body(bodyIndex); body->clearExternalForces(); BodyData& bodyData = bodiesData[bodyIndex]; bodyData.body = body; bodyData.linksData.resize(body->numLinks()); bodyData.hasConstrainedLinks = false; bodyData.isTestForceBeingApplied = false; bodyData.isStatic = body->isStaticModel(); bodyData.forwardDynamicsMM = dynamic_pointer_cast(world.forwardDynamics(bodyIndex)); LinkDataArray& linksData = bodyData.linksData; if(bodyData.isStatic && !(bodyData.forwardDynamicsMM)){ int n = linksData.size(); for(int j=0; j < n; ++j){ LinkData& linkData = linksData[j]; linkData.dw = 0.0; linkData.dvo = 0.0; } } // initialize link data const LinkTraverse& traverse = body->linkTraverse(); for(int j=0; j < traverse.numLinks(); ++j){ Link* link = traverse[j]; linksData[link->index].link = link; linksData[link->index].parentIndex = link->parent ? link->parent->index : -1; } // initialize link connection Body::LinkConnectionArray& connections = body->linkConnections; for(size_t j=0; j < connections.size(); ++j){ connectedLinkPairs.push_back(new LinkPair()); LinkPair& linkPair = *connectedLinkPairs.back(); Body::LinkConnection& connection = connections[j]; linkPair.connection = &connection; linkPair.isSameBodyPair = true; linkPair.constraintPoints.resize(connection.numConstraintAxes); for(int k=0; k < connection.numConstraintAxes; ++k){ ConstraintPoint& constraint = linkPair.constraintPoints[k]; constraint.numFrictionVectors = 0; constraint.globalFrictionIndex = numeric_limits::max(); } for(int k=0; k < 2; ++k){ linkPair.bodyIndex[k] = bodyIndex; linkPair.bodyData[k] = &bodiesData[bodyIndex]; Link* link = connection.link[k]; linkPair.link[k] = link; linkPair.linkData[k] = &(bodyData.linksData[link->index]); } } } int numLinkPairs = collisionCheckLinkPairs.size(); for(int i=0; i < numLinkPairs; ++i){ LinkPair& linkPair = *collisionCheckLinkPairs[i]; for(int j=0; j < 2; ++j){ BodyData& bodyData = bodiesData[linkPair.bodyIndex[j]]; linkPair.bodyData[j] = &bodyData; linkPair.linkData[j] = &(bodyData.linksData[linkPair.link[j]->index]); } } prevGlobalNumConstraintVectors = 0; prevGlobalNumFrictionVectors = 0; numUnconverged = 0; randomAngle.engine().seed(); } inline void CFSImpl::clearExternalForces() { for(size_t i=0; i < bodiesData.size(); ++i){ BodyData& bodyData = bodiesData[i]; if(bodyData.hasConstrainedLinks){ bodyData.body->clearExternalForces(); } } } void CFSImpl::solve(CollisionSequence& corbaCollisionSequence) { if(CFS_DEBUG) std::cout << "Time: " << world.currentTime() << std::endl; for(size_t i=0; i < bodiesData.size(); ++i){ bodiesData[i].hasConstrainedLinks = false; if(useBuiltinCollisionDetector){ bodiesData[i].body->updateLinkColdetModelPositions(); } if(isConstraintForceOutputMode){ BodyPtr& body = bodiesData[i].body; const int n = body->numLinks(); for(int j=0; j < n; ++j){ body->link(j)->constraintForces.clear(); } } } globalNumConstraintVectors = 0; globalNumFrictionVectors = 0; areThereImpacts = false; constrainedLinkPairs.clear(); setConstraintPoints(corbaCollisionSequence); if(CFS_PUT_NUM_CONTACT_POINTS){ cout << globalNumContactNormalVectors; } if(globalNumConstraintVectors > 0){ if(CFS_DEBUG){ std::cout << "Num Collisions: " << globalNumContactNormalVectors << std::endl; } if(CFS_DEBUG_VERBOSE) putContactPoints(); const bool constraintsSizeChanged = ((globalNumFrictionVectors != prevGlobalNumFrictionVectors) || (globalNumConstraintVectors != prevGlobalNumConstraintVectors)); if(constraintsSizeChanged){ initMatrices(); } if(areThereImpacts){ solveImpactConstraints(); } if(SKIP_REDUNDANT_ACCEL_CALC){ setAccelCalcSkipInformation(); } setDefaultAccelerationVector(); setAccelerationMatrix(); clearSingularPointConstraintsOfClosedLoopConnections(); setConstantVectorAndMuBlock(); if(CFS_DEBUG_VERBOSE){ debugPutVector(an0, "an0"); debugPutVector(at0, "at0"); debugPutMatrix(Mlcp, "Mlcp"); vector_range b1(b, range(0, globalNumConstraintVectors)); debugPutVector(b1, "b1"); vector_range b2(b, range(globalNumConstraintVectors, globalNumConstraintVectors + globalNumFrictionVectors)); debugPutVector(b2, "b2"); } bool isConverged; #ifdef USE_PIVOTING_LCP isConverged = callPathLCPSolver(Mlcp, b, solution); #else if(!USE_PREVIOUS_LCP_SOLUTION || constraintsSizeChanged){ solution.clear(); } solveMCPByProjectedGaussSeidel(Mlcp, b, solution); isConverged = true; #endif if(!isConverged){ ++numUnconverged; if(CFS_DEBUG) std::cout << "LCP didn't converge" << numUnconverged << std::endl; } else { if(CFS_DEBUG) std::cout << "LCP converged" << std::endl; if(CFS_DEBUG_LCPCHECK){ // checkLCPResult(Mlcp, b, solution); checkMCPResult(Mlcp, b, solution); } addConstraintForceToLinks(); } } prevGlobalNumConstraintVectors = globalNumConstraintVectors; prevGlobalNumFrictionVectors = globalNumFrictionVectors; } void CFSImpl::setConstraintPoints(CollisionSequence& collisions) { static const bool enableNormalVisualization = true; CollisionPointSequence collisionPoints; CollisionPointSequence* pCollisionPoints = 0; if(useBuiltinCollisionDetector && enableNormalVisualization){ collisions.length(collisionCheckLinkPairs.size()); } for(size_t colIndex=0; colIndex < collisionCheckLinkPairs.size(); ++colIndex){ pCollisionPoints = 0; LinkPair& linkPair = *collisionCheckLinkPairs[colIndex]; if( ! useBuiltinCollisionDetector){ CollisionPointSequence& points = collisions[colIndex].points; if(points.length() > 0){ pCollisionPoints = &points; } } else { if(enableNormalVisualization){ Collision& collision = collisions[colIndex]; collision.pair.charName1 = CORBA::string_dup(linkPair.bodyData[0]->body->name().c_str()); collision.pair.charName2 = CORBA::string_dup(linkPair.bodyData[1]->body->name().c_str()); collision.pair.linkName1 = CORBA::string_dup(linkPair.link[0]->name.c_str()); collision.pair.linkName2 = CORBA::string_dup(linkPair.link[1]->name.c_str()); pCollisionPoints = &collision.points; } else { pCollisionPoints = &collisionPoints; } std::vector& cdata = linkPair.detectCollisions(); if(cdata.empty()){ pCollisionPoints = 0; } else { int npoints = 0; for(int i = 0; i < cdata.size(); i++) { for(int j = 0; j < cdata[i].num_of_i_points; j++){ if(cdata[i].i_point_new[j]) npoints++; } } if(npoints == 0){ pCollisionPoints = 0; } else { pCollisionPoints->length(npoints); int idx = 0; for (int i = 0; i < cdata.size(); i++) { collision_data& cd = cdata[i]; for(int j=0; j < cd.num_of_i_points; j++){ if (cd.i_point_new[j]){ CollisionPoint& point = (*pCollisionPoints)[idx]; for(int k=0; k < 3; k++){ point.position[k] = cd.i_points[j][k]; } for(int k=0; k < 3; k++){ point.normal[k] = cd.n_vector[k]; } point.idepth = cd.depth; idx++; } } } } } } if(pCollisionPoints){ constrainedLinkPairs.push_back(&linkPair); setContactConstraintPoints(linkPair, *pCollisionPoints); linkPair.bodyData[0]->hasConstrainedLinks = true; linkPair.bodyData[1]->hasConstrainedLinks = true; } } globalNumContactNormalVectors = globalNumConstraintVectors; for(size_t i=0; i < connectedLinkPairs.size(); ++i){ LinkPair& linkPair = *connectedLinkPairs[i]; constrainedLinkPairs.push_back(&linkPair); if(setConnectionConstraintPoints(linkPair)){ linkPair.bodyData[0]->hasConstrainedLinks = true; linkPair.bodyData[1]->hasConstrainedLinks = true; } else { constrainedLinkPairs.pop_back(); } } globalNumConnectionVectors = globalNumConstraintVectors - globalNumContactNormalVectors; } void CFSImpl::setContactConstraintPoints(LinkPair& linkPair, CollisionPointSequence& collisionPoints) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; constraintPoints.clear(); int numExtractedPoints = 0; int numContactsInPair = collisionPoints.length(); for(int j=0; j < numContactsInPair; ++j){ CollisionPoint& collision = collisionPoints[j]; constraintPoints.push_back(ConstraintPoint()); ConstraintPoint& contact = constraintPoints.back(); getVector3(contact.point, collision.position); getVector3(contact.normalTowardInside[1], collision.normal); contact.normalTowardInside[0] = -contact.normalTowardInside[1]; contact.depth = collision.idepth; bool isNeighborhood = false; // dense contact points are eliminated for(int k=0; k < numExtractedPoints; ++k){ if(norm2(constraintPoints[k].point - contact.point) < linkPair.culling_thresh){ isNeighborhood = true; break; } } if(isNeighborhood){ constraintPoints.pop_back(); } else { numExtractedPoints++; contact.globalIndex = globalNumConstraintVectors++; // check velocities Vector3 v[2]; for(int k=0; k < 2; ++k){ Link* link = linkPair.link[k]; if(link->isRoot() && link->jointType == Link::FIXED_JOINT){ v[k] = 0.0; } else { v[k] = link->vo + cross(link->w, contact.point); } } contact.relVelocityOn0 = v[1] - v[0]; contact.normalProjectionOfRelVelocityOn0 = dot(contact.normalTowardInside[1], contact.relVelocityOn0); if( ! areThereImpacts){ if(contact.normalProjectionOfRelVelocityOn0 < -1.0e-6){ areThereImpacts = true; } } Vector3 v_tangent(contact.relVelocityOn0 - contact.normalProjectionOfRelVelocityOn0 * contact.normalTowardInside[1]); contact.globalFrictionIndex = globalNumFrictionVectors; double vt_square = dot(v_tangent, v_tangent); static const double vsqrthresh = VEL_THRESH_OF_DYNAMIC_FRICTION * VEL_THRESH_OF_DYNAMIC_FRICTION; bool isSlipping = (vt_square > vsqrthresh); contact.mu = isSlipping ? linkPair.muDynamic : linkPair.muStatic; if( !ONLY_STATIC_FRICTION_FORMULATION && isSlipping){ contact.numFrictionVectors = 1; double vt_mag = sqrt(vt_square); Vector3 t1(v_tangent / vt_mag); Vector3 t2(cross(contact.normalTowardInside[1], t1)); Vector3 t3(cross(t2, contact.normalTowardInside[1])); contact.frictionVector[0][0] = normalize(t3); contact.frictionVector[0][1] = -contact.frictionVector[0][0]; // proportional dynamic friction near zero velocity if(PROPORTIONAL_DYNAMIC_FRICTION){ vt_mag *= 10000.0; if(vt_mag < contact.mu){ contact.mu = vt_mag; } } } else { if(ENABLE_STATIC_FRICTION){ contact.numFrictionVectors = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? 2 : 4); setFrictionVectors(contact); } else { contact.numFrictionVectors = 0; } } globalNumFrictionVectors += contact.numFrictionVectors; } } } void CFSImpl::setFrictionVectors(ConstraintPoint& contact) { Vector3 u(0.0); int minAxis = 0; Vector3& normal = contact.normalTowardInside[0]; for(int i=1; i < 3; i++){ if(fabs(normal(i)) < fabs(normal(minAxis))){ minAxis = i; } } u(minAxis) = 1.0; Vector3 t1(cross(normal, u)); t1 /= norm2(t1); Vector3 t2(cross(normal, t1)); t2 /= norm2(t2); if(ENABLE_RANDOM_STATIC_FRICTION_BASE){ double theta = randomAngle(); contact.frictionVector[0][0] = cos(theta) * t1 + sin(theta) * t2; theta += PI_2; contact.frictionVector[1][0] = cos(theta) * t1 + sin(theta) * t2; } else { contact.frictionVector[0][0] = t1; contact.frictionVector[1][0] = t2; } if(STATIC_FRICTION_BY_TWO_CONSTRAINTS){ contact.frictionVector[0][1] = -contact.frictionVector[0][0]; contact.frictionVector[1][1] = -contact.frictionVector[1][0]; } else { contact.frictionVector[2][0] = -contact.frictionVector[0][0]; contact.frictionVector[3][0] = -contact.frictionVector[1][0]; contact.frictionVector[0][1] = contact.frictionVector[2][0]; contact.frictionVector[1][1] = contact.frictionVector[3][0]; contact.frictionVector[2][1] = contact.frictionVector[0][0]; contact.frictionVector[3][1] = contact.frictionVector[1][0]; } } bool CFSImpl::setConnectionConstraintPoints(LinkPair& linkPair) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; Body::LinkConnection* connection = linkPair.connection; Link* link0 = connection->link[0]; Link* link1 = connection->link[1]; Vector3 point[2]; point[0] = link0->p + link0->R * connection->point[0]; point[1] = link1->p + link1->R * connection->point[1]; Vector3 midPoint((point[0] + point[1]) / 2.0); Vector3 error(midPoint - point[0]); if(dot(error, error) > (0.04 * 0.04)){ return false; } // check velocities Vector3 v[2]; for(int k=0; k < 2; ++k){ Link* link = connection->link[k]; if(link->isRoot() && link->jointType == Link::FIXED_JOINT){ v[k] = 0.0; } else { v[k] = link->vo + cross(link->w, point[k]); } } Vector3 relVelocityOn0(v[1] - v[0]); for(int i=0; i < connection->numConstraintAxes; ++i){ ConstraintPoint& constraint = constraintPoints[i]; constraint.point = midPoint; const Vector3 axis(link0->R * connection->constraintAxes[i]); constraint.normalTowardInside[0] = axis; constraint.normalTowardInside[1] = -axis; constraint.depth = dot(axis, error); constraint.globalIndex = globalNumConstraintVectors++; constraint.normalProjectionOfRelVelocityOn0 = dot(constraint.normalTowardInside[1], relVelocityOn0); } return true; } void CFSImpl::putContactPoints() { std::cout << "Contact Points\n"; for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair* linkPair = constrainedLinkPairs[i]; if(!linkPair->connection){ std::cout << " " << linkPair->link[0]->name << " of " << linkPair->bodyData[0]->body->modelName(); std::cout << "<-->"; std::cout << " " << linkPair->link[1]->name << " of " << linkPair->bodyData[1]->body->modelName(); ConstraintPointArray& constraintPoints = linkPair->constraintPoints; std::cout << "\n"; for(size_t j=0; j < constraintPoints.size(); ++j){ ConstraintPoint& contact = constraintPoints[j]; std::cout << " index " << contact.globalIndex; std::cout << " point: " << contact.point; std::cout << " normal: " << contact.normalTowardInside[1]; std::cout << " rel velocity: " << contact.relVelocityOn0; std::cout << " tangent: " << contact.frictionVector[0]; std::cout << "\n"; } } } std::cout << std::endl; } void CFSImpl::solveImpactConstraints() { if(CFS_DEBUG) std::cout << "Impacts !" << std::endl; } void CFSImpl::initMatrices() { const int n = globalNumConstraintVectors; const int m = globalNumFrictionVectors; const int dimLCP = usePivotingLCP ? (n + m + m) : (n + m); Mlcp.resize(dimLCP, dimLCP, false); b.resize(dimLCP, false); solution.resize(dimLCP, false); range block1(0, n); range block2(n, n + m); if(usePivotingLCP){ range block3(n + m, n + m + m); project(Mlcp, block1, block3) = dzeromatrix(n, m); project(Mlcp, block3, block1) = dzeromatrix(m, n); // clear mu block didentity identity(m); project(Mlcp, block3, block2) = -identity; project(Mlcp, block3, block3) = dzeromatrix(m, m); project(Mlcp, block2, block3) = identity; project(b, block3) = dzerovector(m); } else { frictionIndexToContactIndex.resize(m); contactIndexToMu.resize(globalNumContactNormalVectors, false); mcpHi.resize(globalNumContactNormalVectors, false); } an0.resize(n, false); at0.resize(m, false); } void CFSImpl::setAccelCalcSkipInformation() { // clear skip check numbers for(size_t i=0; i < bodiesData.size(); ++i){ BodyData& bodyData = bodiesData[i]; if(bodyData.hasConstrainedLinks){ LinkDataArray& linksData = bodyData.linksData; for(size_t j=0; j < linksData.size(); ++j){ linksData[j].numberToCheckAccelCalcSkip = numeric_limits::max(); } } } // add the number of contact points to skip check numbers of the links from a contact target to the root int numLinkPairs = constrainedLinkPairs.size(); for(int i=0; i < numLinkPairs; ++i){ LinkPair* linkPair = constrainedLinkPairs[i]; int constraintIndex = linkPair->constraintPoints.front().globalIndex; for(int j=0; j < 2; ++j){ LinkDataArray& linksData = linkPair->bodyData[j]->linksData; int linkIndex = linkPair->link[j]->index; while(linkIndex >= 0){ LinkData& linkData = linksData[linkIndex]; if(linkData.numberToCheckAccelCalcSkip < constraintIndex){ break; } linkData.numberToCheckAccelCalcSkip = constraintIndex; linkIndex = linkData.parentIndex; } } } } void CFSImpl::setDefaultAccelerationVector() { // calculate accelerations with no constraint force for(size_t i=0; i < bodiesData.size(); ++i){ BodyData& bodyData = bodiesData[i]; if(bodyData.hasConstrainedLinks && ! bodyData.isStatic){ if(bodyData.forwardDynamicsMM){ bodyData.rootLinkPosRef = &(bodyData.body->rootLink()->p); Vector3 zeroForce(0.0); bodyData.forwardDynamicsMM->initializeAccelSolver(); bodyData.forwardDynamicsMM->solveUnknownAccels(zeroForce, zeroForce); calcAccelsMM(bodyData, numeric_limits::max()); } else { initABMForceElementsWithNoExtForce(bodyData); calcAccelsABM(bodyData, numeric_limits::max()); } } } // extract accelerations for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair& linkPair = *constrainedLinkPairs[i]; ConstraintPointArray& constraintPoints = linkPair.constraintPoints; for(size_t j=0; j < constraintPoints.size(); ++j){ ConstraintPoint& constraint = constraintPoints[j]; for(int k=0; k < 2; ++k){ if(linkPair.bodyData[k]->isStatic){ constraint.defaultAccel[k] = 0.0; } else { Link* link = linkPair.link[k]; LinkData* linkData = linkPair.linkData[k]; constraint.defaultAccel[k] = linkData->dvo - cross(constraint.point, linkData->dw) + cross(link->w, Vector3(link->vo + cross(link->w, constraint.point))); } } Vector3 relDefaultAccel(constraint.defaultAccel[1] - constraint.defaultAccel[0]); an0[constraint.globalIndex] = dot(constraint.normalTowardInside[1], relDefaultAccel); for(int k=0; k < constraint.numFrictionVectors; ++k){ at0[constraint.globalFrictionIndex + k] = dot(constraint.frictionVector[k][1], relDefaultAccel); } } } } void CFSImpl::setAccelerationMatrix() { const int n = globalNumConstraintVectors; const int m = globalNumFrictionVectors; range block1(0, n); range block2(n, n + m); matrix_range Knn(Mlcp, block1, block1); matrix_range Ktn(Mlcp, block1, block2); matrix_range Knt(Mlcp, block2, block1); matrix_range Ktt(Mlcp, block2, block2); for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair& linkPair = *constrainedLinkPairs[i]; int numConstraintsInPair = linkPair.constraintPoints.size(); for(int j=0; j < numConstraintsInPair; ++j){ ConstraintPoint& constraint = linkPair.constraintPoints[j]; int constraintIndex = constraint.globalIndex; // apply test normal force for(int k=0; k < 2; ++k){ BodyData& bodyData = *linkPair.bodyData[k]; if(!bodyData.isStatic){ bodyData.isTestForceBeingApplied = true; const Vector3& f = constraint.normalTowardInside[k]; if(bodyData.forwardDynamicsMM){ //! \todo This code does not work correctly when the links are in the same body. Fix it. Vector3 arm(constraint.point - *(bodyData.rootLinkPosRef)); Vector3 tau(cross(arm, f)); Vector3 tauext = cross(constraint.point, f); bodyData.forwardDynamicsMM->solveUnknownAccels(linkPair.link[k], f, tauext, f, tau); calcAccelsMM(bodyData, constraintIndex); } else { Vector3 tau(cross(constraint.point, f)); calcABMForceElementsWithTestForce(bodyData, linkPair.link[k], f, tau); if(!linkPair.isSameBodyPair || (k > 0)){ calcAccelsABM(bodyData, constraintIndex); } } } } extractRelAccelsOfConstraintPoints(Knn, Knt, constraintIndex, constraintIndex); // apply test friction force for(int l=0; l < constraint.numFrictionVectors; ++l){ for(int k=0; k < 2; ++k){ BodyData& bodyData = *linkPair.bodyData[k]; if(!bodyData.isStatic){ const Vector3& f = constraint.frictionVector[l][k]; if(bodyData.forwardDynamicsMM){ //! \todo This code does not work correctly when the links are in the same body. Fix it. Vector3 arm(constraint.point - *(bodyData.rootLinkPosRef)); Vector3 tau(cross(arm, f)); Vector3 tauext = cross(constraint.point, f); bodyData.forwardDynamicsMM->solveUnknownAccels(linkPair.link[k], f, tauext, f, tau); calcAccelsMM(bodyData, constraintIndex); } else { Vector3 tau(cross(constraint.point, f)); calcABMForceElementsWithTestForce(bodyData, linkPair.link[k], f, tau); if(!linkPair.isSameBodyPair || (k > 0)){ calcAccelsABM(bodyData, constraintIndex); } } } } extractRelAccelsOfConstraintPoints(Ktn, Ktt, constraint.globalFrictionIndex + l, constraintIndex); } linkPair.bodyData[0]->isTestForceBeingApplied = false; linkPair.bodyData[1]->isTestForceBeingApplied = false; } } if(ASSUME_SYMMETRIC_MATRIX){ copySymmetricElementsOfAccelerationMatrix(Knn, Ktn, Knt, Ktt); } } void CFSImpl::initABMForceElementsWithNoExtForce(BodyData& bodyData) { bodyData.dpf = 0; bodyData.dptau = 0; std::vector& linksData = bodyData.linksData; const LinkTraverse& traverse = bodyData.body->linkTraverse(); int n = traverse.numLinks(); for(int i = n-1; i >= 0; --i){ Link* link = traverse[i]; LinkData& data = linksData[i]; data.pf0 = link->pf; data.ptau0 = link->ptau; for(Link* child = link->child; child; child = child->sibling){ LinkData& childData = linksData[child->index]; data.pf0 += childData.pf0; data.ptau0 += childData.ptau0; if(child->jointType != Link::FIXED_JOINT ){ double uu_dd = childData.uu0 / child->dd; data.pf0 += uu_dd * child->hhv; data.ptau0 += uu_dd * child->hhw; } } if(i > 0){ if(link->jointType != Link::FIXED_JOINT){ data.uu0 = link->uu + link->u - (dot(link->sv, data.pf0) + dot(link->sw, data.ptau0)); data.uu = data.uu0; } } } } void CFSImpl::calcABMForceElementsWithTestForce (BodyData& bodyData, Link* linkToApplyForce, const Vector3& f, const Vector3& tau) { std::vector& linksData = bodyData.linksData; Vector3 dpf (-f); Vector3 dptau(-tau); Link* link = linkToApplyForce; while(link->parent){ if(link->jointType != Link::FIXED_JOINT){ LinkData& data = linksData[link->index]; double duu = -(dot(link->sv, dpf) + dot(link->sw, dptau)); data.uu += duu; double duudd = duu / link->dd; dpf += duudd * link->hhv; dptau += duudd * link->hhw; } link = link->parent; } bodyData.dpf += dpf; bodyData.dptau += dptau; } void CFSImpl::calcAccelsABM(BodyData& bodyData, int constraintIndex) { std::vector& linksData = bodyData.linksData; LinkData& rootData = linksData[0]; Link* rootLink = rootData.link; if(rootLink->jointType == Link::FREE_JOINT){ Vector3 pf (rootData.pf0 + bodyData.dpf); Vector3 ptau(rootData.ptau0 + bodyData.dptau); ublas::bounded_matrix Ia; setMatrix3(rootLink->Ivv, Ia, 0, 0); setTransMatrix3(rootLink->Iwv, Ia, 0, 3); setMatrix3(rootLink->Iwv, Ia, 3, 0); setMatrix3(rootLink->Iww, Ia, 3, 3); ublas::bounded_vector p; setVector3(pf, p, 0); setVector3(ptau, p, 3); p *= -1.0; ublas::permutation_matrix pm(6); ublas::lu_factorize (Ia, pm); ublas::lu_substitute(Ia, pm, p); getVector3(rootData.dvo, p, 0); getVector3(rootData.dw, p, 3); } else { rootData.dw = 0.0; rootData.dvo = 0.0; } // reset bodyData.dpf = 0; bodyData.dptau = 0; int skipCheckNumber = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : (numeric_limits::max() - 1); int n = linksData.size(); for(int linkIndex = 1; linkIndex < n; ++linkIndex){ LinkData& linkData = linksData[linkIndex]; if(!SKIP_REDUNDANT_ACCEL_CALC || linkData.numberToCheckAccelCalcSkip <= skipCheckNumber){ Link* link = linkData.link; LinkData& parentData = linksData[linkData.parentIndex]; if(link->jointType != Link::FIXED_JOINT){ linkData.ddq = (linkData.uu - (dot(link->hhv, parentData.dvo) + dot(link->hhw, parentData.dw))) / link->dd; linkData.dvo = parentData.dvo + link->cv + link->sv * linkData.ddq; linkData.dw = parentData.dw + link->cw + link->sw * linkData.ddq; }else{ linkData.ddq = 0.0; linkData.dvo = parentData.dvo; linkData.dw = parentData.dw; } // reset linkData.uu = linkData.uu0; } } } void CFSImpl::calcAccelsMM(BodyData& bodyData, int constraintIndex) { std::vector& linksData = bodyData.linksData; LinkData& rootData = linksData[0]; Link* rootLink = rootData.link; rootData.dvo = rootLink->dvo; rootData.dw = rootLink->dw; int skipCheckNumber = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : (numeric_limits::max() - 1); int n = linksData.size(); for(int linkIndex = 1; linkIndex < n; ++linkIndex){ LinkData& linkData = linksData[linkIndex]; if(!SKIP_REDUNDANT_ACCEL_CALC || linkData.numberToCheckAccelCalcSkip <= skipCheckNumber){ Link* link = linkData.link; LinkData& parentData = linksData[linkData.parentIndex]; if(link->jointType != Link::FIXED_JOINT){ linkData.dvo = parentData.dvo + link->cv + link->ddq * link->sv; linkData.dw = parentData.dw + link->cw + link->ddq * link->sw; }else{ linkData.dvo = parentData.dvo; linkData.dw = parentData.dw; } } } } void CFSImpl::extractRelAccelsOfConstraintPoints (matrix_range& Kxn, matrix_range& Kxt, int testForceIndex, int constraintIndex) { int maxConstraintIndexToExtract = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : globalNumConstraintVectors; for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair& linkPair = *constrainedLinkPairs[i]; BodyData& bodyData0 = *linkPair.bodyData[0]; BodyData& bodyData1 = *linkPair.bodyData[1]; if(bodyData0.isTestForceBeingApplied){ if(bodyData1.isTestForceBeingApplied){ extractRelAccelsFromLinkPairCase1(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract); } else { extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 0, 1, testForceIndex, maxConstraintIndexToExtract); } } else { if(bodyData1.isTestForceBeingApplied){ extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 1, 0, testForceIndex, maxConstraintIndexToExtract); } else { extractRelAccelsFromLinkPairCase3(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract); } } } } void CFSImpl::extractRelAccelsFromLinkPairCase1 (matrix_range& Kxn, matrix_range& Kxt, LinkPair& linkPair, int testForceIndex, int maxConstraintIndexToExtract) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; for(size_t i=0; i < constraintPoints.size(); ++i){ ConstraintPoint& constraint = constraintPoints[i]; int constraintIndex = constraint.globalIndex; if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){ break; } Link* link0 = linkPair.link[0]; Link* link1 = linkPair.link[1]; LinkData* linkData0 = linkPair.linkData[0]; LinkData* linkData1 = linkPair.linkData[1]; //! \todo Can the follwoing equations be simplified ? Vector3 dv0(linkData0->dvo - cross(constraint.point, linkData0->dw) + cross(link0->w, Vector3(link0->vo + cross(link0->w, constraint.point)))); Vector3 dv1(linkData1->dvo - cross(constraint.point, linkData1->dw) + cross(link1->w, Vector3(link1->vo + cross(link1->w, constraint.point)))); Vector3 relAccel(dv1 - dv0); Kxn(constraintIndex, testForceIndex) = dot(constraint.normalTowardInside[1], relAccel) - an0(constraintIndex); for(int j=0; j < constraint.numFrictionVectors; ++j){ const int index = constraint.globalFrictionIndex + j; Kxt(index, testForceIndex) = dot(constraint.frictionVector[j][1], relAccel) - at0(index); } } } void CFSImpl::extractRelAccelsFromLinkPairCase2 (matrix_range& Kxn, matrix_range& Kxt, LinkPair& linkPair, int iTestForce, int iDefault, int testForceIndex, int maxConstraintIndexToExtract) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; for(size_t i=0; i < constraintPoints.size(); ++i){ ConstraintPoint& constraint = constraintPoints[i]; int constraintIndex = constraint.globalIndex; if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){ break; } Link* link = linkPair.link[iTestForce]; LinkData* linkData = linkPair.linkData[iTestForce]; Vector3 dv(linkData->dvo - cross(constraint.point, linkData->dw) + cross(link->w, Vector3(link->vo + cross(link->w, constraint.point)))); if(CFS_DEBUG_VERBOSE_2) std::cout << "dv " << constraintIndex << " = " << dv << "\n"; Vector3 relAccel(constraint.defaultAccel[iDefault] - dv); Kxn(constraintIndex, testForceIndex) = dot(constraint.normalTowardInside[iDefault], relAccel) - an0(constraintIndex); for(int j=0; j < constraint.numFrictionVectors; ++j){ const int index = constraint.globalFrictionIndex + j; Kxt(index, testForceIndex) = dot(constraint.frictionVector[j][iDefault], relAccel) - at0(index); } } } void CFSImpl::extractRelAccelsFromLinkPairCase3 (matrix_range& Kxn, matrix_range& Kxt, LinkPair& linkPair, int testForceIndex, int maxConstraintIndexToExtract) { ConstraintPointArray& constraintPoints = linkPair.constraintPoints; for(size_t i=0; i < constraintPoints.size(); ++i){ ConstraintPoint& constraint = constraintPoints[i]; int constraintIndex = constraint.globalIndex; if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){ break; } Kxn(constraintIndex, testForceIndex) = 0.0; for(int j=0; j < constraint.numFrictionVectors; ++j){ Kxt(constraint.globalFrictionIndex + j, testForceIndex) = 0.0; } } } void CFSImpl::copySymmetricElementsOfAccelerationMatrix (matrix_range& Knn, matrix_range& Ktn, matrix_range& Knt, matrix_range& Ktt) { for(size_t linkPairIndex=0; linkPairIndex < constrainedLinkPairs.size(); ++linkPairIndex){ ConstraintPointArray& constraintPoints = constrainedLinkPairs[linkPairIndex]->constraintPoints; for(size_t localConstraintIndex = 0; localConstraintIndex < constraintPoints.size(); ++localConstraintIndex){ ConstraintPoint& constraint = constraintPoints[localConstraintIndex]; int constraintIndex = constraint.globalIndex; int nextConstraintIndex = constraintIndex + 1; for(int i = nextConstraintIndex; i < globalNumConstraintVectors; ++i){ Knn(i, constraintIndex) = Knn(constraintIndex, i); } int frictionTopOfNextConstraint = constraint.globalFrictionIndex + constraint.numFrictionVectors; for(int i = frictionTopOfNextConstraint; i < globalNumFrictionVectors; ++i){ Knt(i, constraintIndex) = Ktn(constraintIndex, i); } for(int localFrictionIndex=0; localFrictionIndex < constraint.numFrictionVectors; ++localFrictionIndex){ int frictionIndex = constraint.globalFrictionIndex + localFrictionIndex; for(int i = nextConstraintIndex; i < globalNumConstraintVectors; ++i){ Ktn(i, frictionIndex) = Knt(frictionIndex, i); } for(int i = frictionTopOfNextConstraint; i < globalNumFrictionVectors; ++i){ Ktt(i, frictionIndex) = Ktt(frictionIndex, i); } } } } } void CFSImpl::clearSingularPointConstraintsOfClosedLoopConnections() { for(size_t i = 0; i < Mlcp.size1(); ++i){ if(Mlcp(i, i) < 1.0e-4){ for(size_t j=0; j < Mlcp.size1(); ++j){ Mlcp(j, i) = 0.0; } Mlcp(i, i) = numeric_limits::max(); } } } void CFSImpl::setConstantVectorAndMuBlock() { double dtinv = 1.0 / world.timeStep(); const int block2 = globalNumConstraintVectors; const int block3 = globalNumConstraintVectors + globalNumFrictionVectors; for(size_t i=0; i < constrainedLinkPairs.size(); ++i){ LinkPair& linkPair = *constrainedLinkPairs[i]; int numConstraintsInPair = linkPair.constraintPoints.size(); for(int j=0; j < numConstraintsInPair; ++j){ ConstraintPoint& constraint = linkPair.constraintPoints[j]; int globalIndex = constraint.globalIndex; // set constant vector of LCP // constraints for normal acceleration if(linkPair.connection){ // connection constraint const double& error = constraint.depth; double v; if(error >= 0){ v = 0.1 * (-1.0 + exp(-error * 20.0)); } else { v = 0.1 * ( 1.0 - exp( error * 20.0)); } b(globalIndex) = an0(globalIndex) + (constraint.normalProjectionOfRelVelocityOn0 + v) * dtinv; } else { // contact constraint if(ALLOW_SUBTLE_PENETRATION_FOR_STABILITY){ double extraNegativeVel; double newDepth = ALLOWED_PENETRATION_DEPTH - constraint.depth; extraNegativeVel = NEGATIVE_VELOCITY_RATIO_FOR_PENETRTION * newDepth; b(globalIndex) = an0(globalIndex) + (constraint.normalProjectionOfRelVelocityOn0 + extraNegativeVel) * dtinv; } else { b(globalIndex) = an0(globalIndex) + constraint.normalProjectionOfRelVelocityOn0 * dtinv; } contactIndexToMu[globalIndex] = constraint.mu; int globalFrictionIndex = constraint.globalFrictionIndex; for(int k=0; k < constraint.numFrictionVectors; ++k){ // constraints for tangent acceleration double tangentProjectionOfRelVelocity = dot(constraint.frictionVector[k][1], constraint.relVelocityOn0); b(block2 + globalFrictionIndex) = at0(globalFrictionIndex); if( !IGNORE_CURRENT_VELOCITY_IN_STATIC_FRICTION || constraint.numFrictionVectors == 1){ b(block2 + globalFrictionIndex) += tangentProjectionOfRelVelocity * dtinv; } if(usePivotingLCP){ // set mu (coefficients of friction) Mlcp(block3 + globalFrictionIndex, globalIndex) = constraint.mu; } else { // for iterative solver frictionIndexToContactIndex[globalFrictionIndex] = globalIndex; } ++globalFrictionIndex; } } } } } void CFSImpl::addConstraintForceToLinks() { int n = constrainedLinkPairs.size(); for(int i=0; i < n; ++i){ LinkPair* linkPair = constrainedLinkPairs[i]; for(int j=0; j < 2; ++j){ // if(!linkPair->link[j]->isRoot() || linkPair->link[j]->jointType != Link::FIXED_JOINT){ addConstraintForceToLink(linkPair, j); // } } } } void CFSImpl::addConstraintForceToLink(LinkPair* linkPair, int ipair) { const bool DO_PENALTY_METHOD_TEST = false; //const double kp = 1.0e3; //const double kd = 1.0e2; //const double kp = 1.0e4; //const double kd = 1.0e3; const double kp = 1.0e5; const double kd = 1.0e3; Vector3 f_total(0.0); Vector3 tau_total(0.0); ConstraintPointArray& constraintPoints = linkPair->constraintPoints; int numConstraintPoints = constraintPoints.size(); Link* link = linkPair->link[ipair]; for(int i=0; i < numConstraintPoints; ++i){ ConstraintPoint& constraint = constraintPoints[i]; int globalIndex = constraint.globalIndex; if(!DO_PENALTY_METHOD_TEST){ Vector3 f(solution(globalIndex) * constraint.normalTowardInside[ipair]); for(int j=0; j < constraint.numFrictionVectors; ++j){ f += solution(globalNumConstraintVectors + constraint.globalFrictionIndex + j) * constraint.frictionVector[j][ipair]; } f_total += f; tau_total += cross(constraint.point, f); if(isConstraintForceOutputMode){ Link::ConstraintForceArray& constraintForces = link->constraintForces; constraintForces.resize(constraintForces.size() + 1); Link::ConstraintForce& cforce = constraintForces.back(); cforce.point = constraint.point; cforce.force = f; } } else { double vn((ipair == 0) ? constraint.normalProjectionOfRelVelocityOn0 : -constraint.normalProjectionOfRelVelocityOn0); double fn(constraint.depth * kp - vn * kd); Vector3 f(fn * constraint.normalTowardInside[ipair]); if(fn > 0.0){ //for(int j=0; j < constraint.numFrictionVectors; ++j){ Vector3 v(constraint.relVelocityOn0 - constraint.normalProjectionOfRelVelocityOn0 * constraint.normalTowardInside[1]); //cout << constraint.relVelocityOn0 << " "; if(ipair == 0){ v = -v; } double s = norm2(v); if(s < 1.0e-3){ f += - (v / 1.0e-3) * fn * constraint.mu; } else { f += - (v / s) * fn * constraint.mu; } //} } f_total += f; tau_total += cross(constraint.point, f); } } link->fext += f_total; link->tauext += tau_total; if(CFS_DEBUG){ std::cout << "Constraint force to " << link->name << ": f = " << f_total << ", tau = " << tau_total << std::endl; } } void CFSImpl::solveMCPByProjectedGaussSeidel(const rmdmatrix& M, const dvector& b, dvector& x) { static const int loopBlockSize = DEFAULT_NUM_GAUSS_SEIDEL_ITERATION_BLOCK; if(numGaussSeidelInitialIteration > 0){ solveMCPByProjectedGaussSeidelInitial(M, b, x, numGaussSeidelInitialIteration); } int numBlockLoops = maxNumGaussSeidelIteration / loopBlockSize; if(numBlockLoops==0) numBlockLoops = 1; if(CFS_MCP_DEBUG) cout << "Iteration "; double error = 0.0; int i=0; while(i < numBlockLoops){ i++; solveMCPByProjectedGaussSeidelMain(M, b, x, loopBlockSize - 1); error = solveMCPByProjectedGaussSeidelErrorCheck(M, b, x); if(error < gaussSeidelMaxRelError){ if(CFS_MCP_DEBUG_SHOW_ITERATION_STOP) cout << "stopped at " << i * loopBlockSize << endl; break; } } if(CFS_MCP_DEBUG){ int n = loopBlockSize * i; numGaussSeidelTotalLoops += n; numGaussSeidelTotalCalls++; cout << n; cout << ", avarage = " << (numGaussSeidelTotalLoops / numGaussSeidelTotalCalls); cout << ", error = " << error; cout << endl; } } void CFSImpl::solveMCPByProjectedGaussSeidelInitial (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration) { const int size = globalNumConstraintVectors + globalNumFrictionVectors; const double rstep = 1.0 / (numIteration * size); double r = 0.0; for(int i=0; i < numIteration; ++i){ for(int j=0; j < globalNumContactNormalVectors; ++j){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } const double xx = (-b(j) - sum) / M(j, j); if(xx < 0.0){ x(j) = 0.0; } else { x(j) = r * xx; } r += rstep; mcpHi[j] = contactIndexToMu[j] * x(j); } for(int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } x(j) = r * (-b(j) - sum) / M(j, j); r += rstep; } if(ENABLE_TRUE_FRICTION_CONE){ int contactIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } const double fx0 = (-b(j) - sum) / M(j, j); double& fx = x(j); ++j; sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } const double fy0 = (-b(j) - sum) / M(j, j); double& fy = x(j); const double fmax = mcpHi[contactIndex]; const double fmax2 = fmax * fmax; const double fmag2 = fx0 * fx0 + fy0 * fy0; if(fmag2 > fmax2){ const double s = r * fmax / sqrt(fmag2); fx = s * fx0; fy = s * fy0; } else { fx = r * fx0; fy = r * fy0; } r += (rstep + rstep); } } else { int frictionIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } const double xx = (-b(j) - sum) / M(j, j); const int contactIndex = frictionIndexToContactIndex[frictionIndex]; const double fmax = mcpHi[contactIndex]; const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0); if(xx < fmin){ x(j) = fmin; } else if(xx > fmax){ x(j) = fmax; } else { x(j) = xx; } x(j) *= r; r += rstep; } } } } void CFSImpl::solveMCPByProjectedGaussSeidelMain (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration) { const int size = globalNumConstraintVectors + globalNumFrictionVectors; for(int i=0; i < numIteration; ++i){ for(int j=0; j < globalNumContactNormalVectors; ++j){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } const double xx = (-b(j) - sum) / M(j, j); if(xx < 0.0){ x(j) = 0.0; } else { x(j) = xx; } mcpHi[j] = contactIndexToMu[j] * x(j); } for(int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } x(j) = (-b(j) - sum) / M(j, j); } if(ENABLE_TRUE_FRICTION_CONE){ int contactIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } const double fx0 = (-b(j) - sum) / M(j, j); double& fx = x(j); ++j; sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } const double fy0 = (-b(j) - sum) / M(j, j); double& fy = x(j); const double fmax = mcpHi[contactIndex]; const double fmax2 = fmax * fmax; const double fmag2 = fx0 * fx0 + fy0 * fy0; if(fmag2 > fmax2){ const double s = fmax / sqrt(fmag2); fx = s * fx0; fy = s * fy0; } else { fx = fx0; fy = fy0; } } } else { int frictionIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } const double xx = (-b(j) - sum) / M(j, j); const int contactIndex = frictionIndexToContactIndex[frictionIndex]; const double fmax = mcpHi[contactIndex]; const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0); if(xx < fmin){ x(j) = fmin; } else if(xx > fmax){ x(j) = fmax; } else { x(j) = xx; } } } } } double CFSImpl::solveMCPByProjectedGaussSeidelErrorCheck (const rmdmatrix& M, const dvector& b, dvector& x) { const int size = globalNumConstraintVectors + globalNumFrictionVectors; double error = 0.0; for(int j=0; j < globalNumConstraintVectors; ++j){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } double xx = (-b(j) - sum) / M(j, j); if(j < globalNumContactNormalVectors){ if(xx < 0.0){ xx = 0.0; } mcpHi[j] = contactIndexToMu[j] * xx; } double d = fabs(xx - x(j)); if(xx > numeric_limits::epsilon()){ d /= xx; } if(d > error){ error = d; } x(j) = xx; } if(ENABLE_TRUE_FRICTION_CONE){ int contactIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } double fx0 = (-b(j) - sum) / M(j, j); double& fx = x(j); ++j; sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } double fy0 = (-b(j) - sum) / M(j, j); double& fy = x(j); const double fmax = mcpHi[contactIndex]; const double fmax2 = fmax * fmax; const double fmag2 = fx0 * fx0 + fy0 * fy0; if(fmag2 > fmax2){ const double s = fmax / sqrt(fmag2); fx0 *= s; fy0 *= s; } double d = fabs(fx0 - fx); const double afx0 = fabs(fx0); if(afx0 > numeric_limits::epsilon()){ d /= afx0; } if(d > error){ error = d; } d = fabs(fy0 - fy); const double afy0 = fabs(fy0); if(afy0 > numeric_limits::epsilon()){ d /= afy0; } if(d > error){ error = d; } fx = fx0; fy = fy0; } } else { int frictionIndex = 0; for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){ double sum = -M(j, j) * x(j); for(int k=0; k < size; ++k){ sum += M(j, k) * x(k); } double xx = (-b(j) - sum) / M(j, j); const int contactIndex = frictionIndexToContactIndex[frictionIndex]; const double fmax = mcpHi[contactIndex]; const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0); if(xx < fmin){ xx = fmin; } else if(xx > fmax){ xx = fmax; } double d = fabs(xx - x(j)); if(xx > numeric_limits::epsilon()){ d /= xx; } if(d > error){ error = d; } x(j) = xx; } } return error; } void CFSImpl::checkLCPResult(rmdmatrix& M, dvector& b, dvector& x) { std::cout << "check LCP result\n"; std::cout << "-------------------------------\n"; dvector z = prod(M, x) + b; int n = x.size(); for(int i=0; i < n; ++i){ std::cout << "(" << x(i) << ", " << z(i) << ")"; if(x(i) < 0.0 || z(i) < 0.0 || x(i) * z(i) != 0.0){ std::cout << " - X"; } std::cout << "\n"; if(i == globalNumConstraintVectors){ std::cout << "-------------------------------\n"; } else if(i == globalNumConstraintVectors + globalNumFrictionVectors){ std::cout << "-------------------------------\n"; } } std::cout << "-------------------------------\n"; std::cout << std::endl; } void CFSImpl::checkMCPResult(rmdmatrix& M, dvector& b, dvector& x) { std::cout << "check MCP result\n"; std::cout << "-------------------------------\n"; dvector z = prod(M, x) + b; for(int i=0; i < globalNumConstraintVectors; ++i){ std::cout << "(" << x(i) << ", " << z(i) << ")"; if(x(i) < 0.0 || z(i) < -1.0e-6){ std::cout << " - X"; } else if(x(i) > 0.0 && fabs(z(i)) > 1.0e-6){ std::cout << " - X"; } else if(z(i) > 1.0e-6 && fabs(x(i)) > 1.0e-6){ std::cout << " - X"; } std::cout << "\n"; } std::cout << "-------------------------------\n"; int j = 0; for(int i=globalNumConstraintVectors; i < globalNumConstraintVectors + globalNumFrictionVectors; ++i, ++j){ std::cout << "(" << x(i) << ", " << z(i) << ")"; int contactIndex = frictionIndexToContactIndex[j]; double hi = contactIndexToMu[contactIndex] * x(contactIndex); std::cout << " hi = " << hi; if(x(i) < 0.0 || x(i) > hi){ std::cout << " - X"; } else if(x(i) == hi && z(i) > -1.0e-6){ std::cout << " - X"; } else if(x(i) < hi && x(i) > 0.0 && fabs(z(i)) > 1.0e-6){ std::cout << " - X"; } std::cout << "\n"; } std::cout << "-------------------------------\n"; std::cout << std::endl; } #ifdef USE_PIVOTING_LCP bool CFSImpl::callPathLCPSolver(rmdmatrix& Mlcp, dvector& b, dvector& solution) { int size = solution.size(); int square = size * size; std::vector lb(size + 1, 0.0); std::vector ub(size + 1, 1.0e20); int m_nnz = 0; std::vector m_i(square + 1); std::vector m_j(square + 1); std::vector m_ij(square + 1); for(int i=0; i < size; ++i){ solution(i) = 0.0; } for(int j=0; j < size; ++j){ for(int i=0; i < size; ++i){ double v = Mlcp(i, j); if(v != 0.0){ m_i[m_nnz] = i+1; m_j[m_nnz] = j+1; m_ij[m_nnz] = v; ++m_nnz; } } } MCP_Termination status; SimpleLCP(size, m_nnz, &m_i[0], &m_j[0], &m_ij[0], &b(0), &lb[0], &ub[0], &status, &solution(0)); return (status == MCP_Solved); } #endif ConstraintForceSolver::ConstraintForceSolver(WorldBase& world) { impl = new CFSImpl(world); } ConstraintForceSolver::~ConstraintForceSolver() { delete impl; } bool ConstraintForceSolver::addCollisionCheckLinkPair (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double epsilon) { return impl->addCollisionCheckLinkPair(bodyIndex1, link1, bodyIndex2, link2, muStatic, muDynamic, culling_thresh, epsilon); } void ConstraintForceSolver::clearCollisionCheckLinkPairs() { impl->world.clearCollisionPairs(); impl->collisionCheckLinkPairs.clear(); } void ConstraintForceSolver::setGaussSeidelParameters(int maxNumIteration, int numInitialIteration, double maxRelError) { impl->maxNumGaussSeidelIteration = maxNumIteration; impl->numGaussSeidelInitialIteration = numInitialIteration; impl->gaussSeidelMaxRelError = maxRelError; } void ConstraintForceSolver::enableConstraintForceOutput(bool on) { impl->isConstraintForceOutputMode = on; } void ConstraintForceSolver::useBuiltinCollisionDetector(bool on) { impl->useBuiltinCollisionDetector = on; } void ConstraintForceSolver::initialize(void) { impl->initialize(); } void ConstraintForceSolver::solve(CollisionSequence& corbaCollisionSequence) { impl->solve(corbaCollisionSequence); } void ConstraintForceSolver::clearExternalForces() { impl->clearExternalForces(); } choreonoid-1.1.0+dfsg/src/Body/ForwardDynamics.cpp000066400000000000000000000100511207742442300220470ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "ForwardDynamics.h" #include "Body.h" #include "Link.h" #include "Sensor.h" using namespace cnoid; ForwardDynamics::ForwardDynamics(BodyPtr body) : body(body) { g.setZero(); timeStep = 0.005; integrationMode = RUNGEKUTTA_METHOD; sensorsEnabled = false; } ForwardDynamics::~ForwardDynamics() { } void ForwardDynamics::setTimeStep(double ts) { timeStep = ts; } void ForwardDynamics::setGravityAcceleration(const Vector3& g) { this->g = g; } void ForwardDynamics::setEulerMethod() { integrationMode = EULER_METHOD; } void ForwardDynamics::setRungeKuttaMethod() { integrationMode = RUNGEKUTTA_METHOD; } void ForwardDynamics::enableSensors(bool on) { sensorsEnabled = on; } /// function from Murray, Li and Sastry p.42 void ForwardDynamics::SE3exp (Vector3& out_p, Matrix3& out_R, const Vector3& p0, const Matrix3& R0, const Vector3& w, const Vector3& vo, double dt) { double norm_w = w.norm(); if(norm_w < std::numeric_limits::epsilon()) { out_p = p0 + vo * dt; out_R = R0; } else { double th = norm_w * dt; Vector3 w_n = w / norm_w; Vector3 vo_n = vo / norm_w; Matrix3 rot(AngleAxisd(th, w_n)); out_p.noalias() = rot * p0 + (Matrix3::Identity() - rot) * w_n.cross(vo_n) + (w_n * w_n.transpose()) * vo_n * th; out_R.noalias() = rot * R0; } } void ForwardDynamics::initializeSensors() { body->clearSensorValues(); if(sensorsEnabled){ initializeAccelSensors(); } } void ForwardDynamics::updateSensorsFinal() { int n; n = body->numSensors(Sensor::RATE_GYRO); for(int i=0; i < n; ++i){ RateGyroSensor* sensor = body->sensor(i); Link* link = sensor->link; sensor->w = sensor->localR.transpose() * link->R.transpose() * link->w; } n = body->numSensors(Sensor::ACCELERATION); for(int i=0; i < n; ++i){ updateAccelSensor(body->sensor(i)); } } void ForwardDynamics::updateAccelSensor(AccelSensor* sensor) { Link* link = sensor->link; Vector2* x = sensor->x; Vector3 o_Vgsens = link->R * (link->R.transpose() * link->w).cross(sensor->localPos) + link->v; if(sensor->isFirstUpdate){ sensor->isFirstUpdate = false; for(int i=0; i < 3; ++i){ x[i](0) = o_Vgsens(i); x[i](1) = 0.0; } } else { // kalman filtering for(int i=0; i < 3; ++i){ x[i] = A * x[i] + o_Vgsens(i) * B; } } Vector3 o_Agsens(x[0](1), x[1](1), x[2](1)); o_Agsens += g; sensor->dv.noalias() = link->R.transpose() * o_Agsens; } void ForwardDynamics::initializeAccelSensors() { int n = body->numSensors(Sensor::ACCELERATION); if(n > 0){ for(int i=0; i < n; ++i){ AccelSensor* sensor = body->sensor(i); if(sensor){ sensor->isFirstUpdate = true; } } // Kalman filter design static const double n_input = 100.0; // [N] static const double n_output = 0.001; // [m/s] // Analytical solution of Kalman filter (continuous domain) // s.kajita 2003 Jan.22 Matrix2 Ac; Ac << -sqrt(2.0 * n_input / n_output), 1.0, -n_input / n_output , 0.0; Vector2 Bc(sqrt(2.0 * n_input / n_output), n_input / n_output); A.setIdentity(); Matrix2 An = Matrix2::Identity(); Matrix2 An2; B = timeStep * Bc; Vector2 Bn = B; Vector2 Bn2; double factorial[14]; double r = 1.0; factorial[1] = r; for(int i=2; i <= 13; ++i){ r += 1.0; factorial[i] = factorial[i-1] * r; } for(int i=1; i <= 12; i++){ An2.noalias() = Ac * An; An = timeStep * An2; A += (1.0 / factorial[i]) * An; Bn2.noalias() = Ac * Bn; Bn = timeStep * Bn2; B += (1.0 / factorial[i+1]) * Bn; } } } choreonoid-1.1.0+dfsg/src/Body/ForwardDynamics.h000066400000000000000000000041231207742442300215170ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_FORWARD_DYNAMICS_H_INCLUDED #define CNOID_BODY_FORWARD_DYNAMICS_H_INCLUDED #include "Body.h" #include #include #include "exportdecl.h" namespace cnoid { class AccelSensor; /** This class calculates the forward dynamics of a Body object by using the Featherstone's articulated body algorithm. The class also integrates motion using the Euler method or RungeKutta method. */ class CNOID_EXPORT ForwardDynamics { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ForwardDynamics(BodyPtr body); virtual ~ForwardDynamics(); void setGravityAcceleration(const Vector3& g); void setEulerMethod(); void setRungeKuttaMethod(); void setTimeStep(double timeStep); void enableSensors(bool on); virtual void initialize() = 0; virtual void calcNextState() = 0; protected: virtual void initializeSensors(); virtual void updateSensorsFinal(); /** @brief update position/orientation using spatial velocity @param out_p p(t+dt) @param out_R R(t+dt) @param p0 p(t) @param R0 R(t) @param w angular velocity @param v0 spatial velocity @param dt time step[s] */ static void SE3exp(Vector3& out_p, Matrix3& out_R, const Vector3& p0, const Matrix3& R0, const Vector3& w, const Vector3& vo, double dt); BodyPtr body; Vector3 g; double timeStep; bool sensorsEnabled; enum { EULER_METHOD, RUNGEKUTTA_METHOD } integrationMode; private: void updateAccelSensor(AccelSensor* sensor); void initializeAccelSensors(); // varialbes for calculating sensor values // preview control gain matrices for force sensors Matrix2 A; Vector2 B; }; typedef boost::shared_ptr ForwardDynamicsPtr; }; #endif choreonoid-1.1.0+dfsg/src/Body/ForwardDynamicsABM.cpp000066400000000000000000000336041207742442300224000ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "ForwardDynamicsABM.h" #include "Body.h" #include "Link.h" #include "LinkTraverse.h" #include "Sensor.h" #include using namespace std; using namespace cnoid; static const bool debugMode = false; static const bool rootAttitudeNormalizationEnabled = false; ForwardDynamicsABM::ForwardDynamicsABM(BodyPtr body) : ForwardDynamics(body), q0(body->numLinks()), dq0(body->numLinks()), dq(body->numLinks()), ddq(body->numLinks()) { } ForwardDynamicsABM::~ForwardDynamicsABM() { } void ForwardDynamicsABM::initialize() { initializeSensors(); calcABMFirstHalf(); } inline void ForwardDynamicsABM::calcABMFirstHalf() { calcABMPhase1(); calcABMPhase2Part1(); } inline void ForwardDynamicsABM::calcABMLastHalf() { calcABMPhase2Part2(); calcABMPhase3(); } void ForwardDynamicsABM::calcNextState() { switch(integrationMode){ case EULER_METHOD: calcMotionWithEulerMethod(); break; case RUNGEKUTTA_METHOD: calcMotionWithRungeKuttaMethod(); break; } if(rootAttitudeNormalizationEnabled){ Matrix3& R = body->rootLink()->R; Matrix3::ColXpr x = R.col(0); Matrix3::ColXpr y = R.col(1); Matrix3::ColXpr z = R.col(2); x.normalize(); z = x.cross(y).normalized(); y = z.cross(x); } calcABMFirstHalf(); if(sensorsEnabled){ updateSensorsFinal(); } body->setVirtualJointForces(); } void ForwardDynamicsABM::calcMotionWithEulerMethod() { calcABMLastHalf(); if(sensorsEnabled){ updateForceSensors(); } Link* root = body->rootLink(); if(root->jointType != Link::FIXED_JOINT){ Vector3 p; Matrix3 R; SE3exp(p, R, root->p, root->R, root->w, root->vo, timeStep); root->p = p; root->R = R; root->vo += root->dvo * timeStep; root->w += root->dw * timeStep; } int n = body->numLinks(); for(int i=1; i < n; ++i){ Link* link = body->link(i); link->q += link->dq * timeStep; link->dq += link->ddq * timeStep; } } void ForwardDynamicsABM::integrateRungeKuttaOneStep(double r, double dt) { Link* root = body->rootLink(); if(root->jointType != Link::FIXED_JOINT){ SE3exp(root->p, root->R, p0, R0, root->w, root->vo, dt); root->vo = vo0 + root->dvo * dt; root->w = w0 + root->dw * dt; vo += r * root->vo; w += r * root->w; dvo += r * root->dvo; dw += r * root->dw; } int n = body->numLinks(); for(int i=1; i < n; ++i){ Link* link = body->link(i); link->q = q0[i] + dt * link->dq; link->dq = dq0[i] + dt * link->ddq; dq[i] += r * link->dq; ddq[i] += r * link->ddq; } } void ForwardDynamicsABM::calcMotionWithRungeKuttaMethod() { Link* root = body->rootLink(); if(root->jointType != Link::FIXED_JOINT){ p0 = root->p; R0 = root->R; vo0 = root->vo; w0 = root->w; } vo.setZero(); w.setZero(); dvo.setZero(); dw.setZero(); int n = body->numLinks(); for(int i=1; i < n; ++i){ Link* link = body->link(i); q0[i] = link->q; dq0[i] = link->dq; dq[i] = 0.0; ddq[i] = 0.0; } calcABMLastHalf(); if(sensorsEnabled){ updateForceSensors(); } integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0); calcABMPhase1(); calcABMPhase2(); calcABMPhase3(); integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0); calcABMPhase1(); calcABMPhase2(); calcABMPhase3(); integrateRungeKuttaOneStep(2.0 / 6.0, timeStep); calcABMPhase1(); calcABMPhase2(); calcABMPhase3(); if(root->jointType != Link::FIXED_JOINT){ SE3exp(root->p, root->R, p0, R0, w0, vo0, timeStep); root->vo = vo0 + (dvo + root->dvo / 6.0) * timeStep; root->w = w0 + (dw + root->dw / 6.0) * timeStep; } for(int i=1; i < n; ++i){ Link* link = body->link(i); link->q = q0[i] + ( dq[i] + link->dq / 6.0) * timeStep; link->dq = dq0[i] + (ddq[i] + link->ddq / 6.0) * timeStep; } } void ForwardDynamicsABM::calcABMPhase1() { const LinkTraverse& traverse = body->linkTraverse(); int n = traverse.numLinks(); for(int i=0; i < n; ++i){ Link* link = traverse[i]; Link* parent = link->parent; if(parent){ switch(link->jointType){ case Link::ROTATIONAL_JOINT: link->R.noalias() = parent->R * AngleAxisd(link->q, link->a); link->p.noalias() = parent->R * link->b + parent->p; link->sw.noalias() = parent->R * link->a; link->sv = link->p.cross(link->sw); link->w = link->dq * link->sw + parent->w; break; case Link::SLIDE_JOINT: link->p.noalias() = parent->R * (link->b + link->q * link->d) + parent->p; link->R = parent->R; link->sw.setZero(); link->sv.noalias() = parent->R * link->d; link->w = parent->w; break; case Link::FIXED_JOINT: default: link->p.noalias() = parent->R * link->b + parent->p; link->R = parent->R; link->w = parent->w; link->vo = parent->vo; link->sw.setZero(); link->sv.setZero(); link->cv.setZero(); link->cw.setZero(); goto COMMON_CALCS_FOR_ALL_JOINT_TYPES; } // Common for ROTATE and SLIDE link->vo = link->dq * link->sv + parent->vo; const Vector3 dsv = parent->w.cross(link->sv) + parent->vo.cross(link->sw); const Vector3 dsw = parent->w.cross(link->sw); link->cv = link->dq * dsv; link->cw = link->dq * dsw; } COMMON_CALCS_FOR_ALL_JOINT_TYPES: link->v = link->vo + link->w.cross(link->p); link->wc.noalias() = link->R * link->c + link->p; // compute I^s (Eq.(6.24) of Kajita's textbook)) const Matrix3 Iw = link->R * link->I * link->R.transpose(); const Matrix3 c_hat = hat(link->wc); link->Iww.noalias() = link->m * c_hat * c_hat.transpose() + Iw; link->Ivv << link->m, 0.0, 0.0, 0.0, link->m, 0.0, 0.0, 0.0, link->m; link->Iwv = link->m * c_hat; // compute P and L (Eq.(6.25) of Kajita's textbook) const Vector3 P = link->m * (link->vo + link->w.cross(link->wc)); const Vector3 L = link->Iww * link->w + link->m * link->wc.cross(link->vo); link->pf = link->w.cross(P); link->ptau = link->vo.cross(P) + link->w.cross(L); const Vector3 fg = -link->m * g; const Vector3 tg = link->wc.cross(fg); link->pf -= fg; link->ptau -= tg; } } void ForwardDynamicsABM::calcABMPhase2() { const LinkTraverse& traverse = body->linkTraverse(); int n = traverse.numLinks(); for(int i = n-1; i >= 0; --i){ Link* link = traverse[i]; link->pf -= link->fext; link->ptau -= link->tauext; // compute articulated inertia (Eq.(6.48) of Kajita's textbook) for(Link* child = link->child; child; child = child->sibling){ if(child->jointType == Link::FIXED_JOINT){ link->Ivv += child->Ivv; link->Iwv += child->Iwv; link->Iww += child->Iww; }else{ const Vector3 hhv_dd = child->hhv / child->dd; link->Ivv.noalias() += child->Ivv - child->hhv * hhv_dd.transpose(); link->Iwv.noalias() += child->Iwv - child->hhw * hhv_dd.transpose(); link->Iww.noalias() += child->Iww - child->hhw * (child->hhw / child->dd).transpose(); } link->pf .noalias() += child->Ivv * child->cv + child->Iwv.transpose() * child->cw + child->pf; link->ptau.noalias() += child->Iwv * child->cv + child->Iww * child->cw + child->ptau; if(child->jointType != Link::FIXED_JOINT){ const double uu_dd = child->uu / child->dd; link->pf += uu_dd * child->hhv; link->ptau += uu_dd * child->hhw; } } if(i > 0){ if(link->jointType != Link::FIXED_JOINT){ // hh = Ia * s link->hhv.noalias() = link->Ivv * link->sv + link->Iwv.transpose() * link->sw; link->hhw.noalias() = link->Iwv * link->sv + link->Iww * link->sw; // dd = Ia * s * s^T link->dd = link->sv.dot(link->hhv) + link->sw.dot(link->hhw) + link->Jm2; // uu = u - hh^T*c + s^T*pp link->uu = link->u - (link->hhv.dot(link->cv) + link->hhw.dot(link->cw) + link->sv.dot(link->pf) + link->sw.dot(link->ptau)); } } } } // A part of phase 2 (inbound loop) that can be calculated before external forces are given void ForwardDynamicsABM::calcABMPhase2Part1() { const LinkTraverse& traverse = body->linkTraverse(); int n = traverse.numLinks(); for(int i = n-1; i >= 0; --i){ Link* link = traverse[i]; for(Link* child = link->child; child; child = child->sibling){ if(child->jointType == Link::FIXED_JOINT){ link->Ivv += child->Ivv; link->Iwv += child->Iwv; link->Iww += child->Iww; }else{ const Vector3 hhv_dd = child->hhv / child->dd; link->Ivv.noalias() += child->Ivv - child->hhv * hhv_dd.transpose(); link->Iwv.noalias() += child->Iwv - child->hhw * hhv_dd.transpose(); link->Iww.noalias() += child->Iww - child->hhw * (child->hhw / child->dd).transpose(); } link->pf .noalias() += child->Ivv * child->cv + child->Iwv.transpose() * child->cw; link->ptau.noalias() += child->Iwv * child->cv + child->Iww * child->cw; } if(i > 0){ if(link->jointType != Link::FIXED_JOINT){ link->hhv.noalias() = link->Ivv * link->sv + link->Iwv.transpose() * link->sw; link->hhw.noalias() = link->Iwv * link->sv + link->Iww * link->sw; link->dd = link->sv.dot(link->hhv) + link->sw.dot(link->hhw) + link->Jm2; link->uu = -(link->hhv.dot(link->cv) + link->hhw.dot(link->cw)); } } } } // A remaining part of phase 2 that requires external forces void ForwardDynamicsABM::calcABMPhase2Part2() { const LinkTraverse& traverse = body->linkTraverse(); int n = traverse.numLinks(); for(int i = n-1; i >= 0; --i){ Link* link = traverse[i]; link->pf -= link->fext; link->ptau -= link->tauext; for(Link* child = link->child; child; child = child->sibling){ link->pf += child->pf; link->ptau += child->ptau; if(child->jointType != Link::FIXED_JOINT){ const double uu_dd = child->uu / child->dd; link->pf += uu_dd * child->hhv; link->ptau += uu_dd * child->hhw; } } if(i > 0){ if(link->jointType != Link::FIXED_JOINT) link->uu += link->u - (link->sv.dot(link->pf) + link->sw.dot(link->ptau)); } } } void ForwardDynamicsABM::calcABMPhase3() { const LinkTraverse& traverse = body->linkTraverse(); Link* root = traverse[0]; if(root->jointType == Link::FREE_JOINT){ // - | Ivv trans(Iwv) | * | dvo | = | pf | // | Iwv Iww | | dw | | ptau | Eigen::Matrix M; M << root->Ivv, root->Iwv.transpose(), root->Iwv, root->Iww; Eigen::Matrix f; f << root->pf, root->ptau; f *= -1.0; Eigen::Matrix a(M.colPivHouseholderQr().solve(f)); root->dvo = a.head<3>(); root->dw = a.tail<3>(); } else { root->dvo.setZero(); root->dw.setZero(); } int n = traverse.numLinks(); for(int i=1; i < n; ++i){ Link* link = traverse[i]; Link* parent = link->parent; if(link->jointType != Link::FIXED_JOINT){ link->ddq = (link->uu - (link->hhv.dot(parent->dvo) + link->hhw.dot(parent->dw))) / link->dd; link->dvo = parent->dvo + link->cv + link->sv * link->ddq; link->dw = parent->dw + link->cw + link->sw * link->ddq; }else{ link->ddq = 0.0; link->dvo = parent->dvo; link->dw = parent->dw; } } } void ForwardDynamicsABM::updateForceSensors() { int n = body->numSensors(Sensor::FORCE); for(int i=0; i < n; ++i){ updateForceSensor(body->sensor(i)); } } void ForwardDynamicsABM::updateForceSensor(ForceSensor* sensor) { Link* link = sensor->link; // | f | = | Ivv trans(Iwv) | * | dvo | + | pf | // | tau | | Iwv Iww | | dw | | ptau | const Vector3 f = -(link->Ivv * link->dvo + link->Iwv.transpose() * link->dw + link->pf); const Vector3 tau = -(link->Iwv * link->dvo + link->Iww * link->dw + link->ptau); const Matrix3 sensorR = link->R * sensor->localR; const Vector3 fs = sensorR.transpose() * f; const Vector3 sensorPos = link->p + link->R * sensor->localPos; const Vector3 ts = sensorR.transpose() * (tau - sensorPos.cross(f)); sensor->f = fs; sensor->tau = ts; if(debugMode){ cout << "sensor " << sensor->name << ": "; cout << "f = " << f; cout << "R = " << sensorR; cout << "sensor->f = " << sensor->f; cout << endl; } } choreonoid-1.1.0+dfsg/src/Body/ForwardDynamicsABM.h000066400000000000000000000033651207742442300220460ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_FORWARD_DYNAMICS_ABM_H_INCLUDED #define CNOID_BODY_FORWARD_DYNAMICS_ABM_H_INCLUDED #include "ForwardDynamics.h" #include #include #include "exportdecl.h" namespace cnoid { class LinkTraverse; class AccelSensor; class ForceSensor; /** Forward dynamics calculation using Featherstone's Articulated Body Method (ABM) */ class CNOID_EXPORT ForwardDynamicsABM : public ForwardDynamics { public: ForwardDynamicsABM(BodyPtr body); ~ForwardDynamicsABM(); virtual void initialize(); virtual void calcNextState(); private: void calcMotionWithEulerMethod(); void integrateRungeKuttaOneStep(double r, double dt); void calcMotionWithRungeKuttaMethod(); /** compute position/orientation/velocity */ void calcABMPhase1(); /** compute articulated inertia */ void calcABMPhase2(); void calcABMPhase2Part1(); void calcABMPhase2Part2(); /** compute joint acceleration/spatial acceleration */ void calcABMPhase3(); inline void calcABMFirstHalf(); inline void calcABMLastHalf(); void updateForceSensors(); void updateForceSensor(ForceSensor* sensor); // Buffers for the Runge Kutta Method Vector3 p0; Matrix3 R0; Vector3 vo0; Vector3 w0; std::vector q0; std::vector dq0; Vector3 vo; Vector3 w; Vector3 dvo; Vector3 dw; std::vector dq; std::vector ddq; }; }; #endif choreonoid-1.1.0+dfsg/src/Body/ForwardDynamicsCBM.cpp000066400000000000000000000511711207742442300224010ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "Body.h" #include "Link.h" #include "LinkTraverse.h" #include "Sensor.h" #include "ForwardDynamicsCBM.h" #include using namespace std; using namespace cnoid; static const bool CALC_ALL_JOINT_TORQUES = false; static const bool ROOT_ATT_NORMALIZATION_ENABLED = false; static const bool debugMode = false; #include template static void putMatrix(TMatrix& M, char* name) { if(M.cols() == 1){ std::cout << "Vector " << name << M << std::endl; } else { std::cout << "Matrix " << name << ": \n"; for(size_t i=0; i < M.rows(); i++){ for(size_t j=0; j < M.cols(); j++){ std::cout << boost::format(" %6.3f ") % M(i, j); } std::cout << std::endl; } } } template static void putVector(TVector& M, char* name) { std::cout << "Vector " << name << M << std::endl; } ForwardDynamicsMM::ForwardDynamicsMM(BodyPtr body) : ForwardDynamics(body) { } ForwardDynamicsMM::~ForwardDynamicsMM() { } void ForwardDynamicsMM::initialize() { Link* root = body->rootLink(); unknown_rootDof = (root->jointType == Link::FREE_JOINT && !root->isHighGainMode ) ? 6 : 0; given_rootDof = (root->jointType == Link::FREE_JOINT && root->isHighGainMode ) ? 6 : 0; torqueModeJoints.clear(); highGainModeJoints.clear(); int numLinks = body->numLinks(); for(int i=1; i < numLinks; ++i){ Link* link = body->link(i); int jointType = link->jointType; if(jointType == Link::ROTATIONAL_JOINT || jointType == Link::SLIDE_JOINT){ if(link->isHighGainMode){ highGainModeJoints.push_back(link); } else { torqueModeJoints.push_back(link); } } } int n = unknown_rootDof + torqueModeJoints.size(); int m = highGainModeJoints.size(); isNoUnknownAccelMode = (n == 0); M11.resize(n, n); M12.resize(n, given_rootDof + m); b1. resize(n, 1); c1. resize(n); d1. resize(n); qGiven. resize(m); dqGiven. resize(m); ddqGiven.resize(given_rootDof + m); qGivenPrev. resize(m); dqGivenPrev.resize(m); ddqorg.resize(numLinks); uorg. resize(numLinks); calcPositionAndVelocityFK(); if(!isNoUnknownAccelMode){ calcMassMatrix(); } ddqGivenCopied = false; initializeSensors(); if(integrationMode == RUNGEKUTTA_METHOD){ q0. resize(numLinks); dq0.resize(numLinks); dq. resize(numLinks); ddq.resize(numLinks); preserveHighGainModeJointState(); } } inline void ForwardDynamicsMM::solveUnknownAccels() { if(!isNoUnknownAccelMode){ initializeAccelSolver(); solveUnknownAccels(fextTotal, tauextTotal); } } inline void ForwardDynamicsMM::calcAccelFKandForceSensorValues() { Vector3 f, tau; calcAccelFKandForceSensorValues(body->rootLink(), f, tau); } void ForwardDynamicsMM::calcNextState() { if(isNoUnknownAccelMode && !body->numSensors(Sensor::FORCE)){ calcPositionAndVelocityFK(); } else { switch(integrationMode){ case EULER_METHOD: calcMotionWithEulerMethod(); break; case RUNGEKUTTA_METHOD: calcMotionWithRungeKuttaMethod(); break; } if(ROOT_ATT_NORMALIZATION_ENABLED && unknown_rootDof){ Matrix3& R = body->rootLink()->R; Matrix3::ColXpr x = R.col(0); Matrix3::ColXpr y = R.col(1); Matrix3::ColXpr z = R.col(2); x.normalize(); z = x.cross(y).normalized(); y = z.cross(x); } } if(sensorsEnabled){ updateSensorsFinal(); } body->setVirtualJointForces(); ddqGivenCopied = false; } void ForwardDynamicsMM::calcMotionWithEulerMethod() { sumExternalForces(); solveUnknownAccels(); calcAccelFKandForceSensorValues(); Link* root = body->rootLink(); if(unknown_rootDof){ Vector3 p; Matrix3 R; SE3exp(p, R, root->p, root->R, root->w, root->vo, timeStep); root->p = p; root->R = R; root->vo += root->dvo * timeStep; root->w += root->dw * timeStep; } int n = torqueModeJoints.size(); for(int i=0; i < n; ++i){ Link* link = torqueModeJoints[i]; link->q += link->dq * timeStep; link->dq += link->ddq * timeStep; } calcPositionAndVelocityFK(); calcMassMatrix(); } void ForwardDynamicsMM::calcMotionWithRungeKuttaMethod() { int numHighGainJoints = highGainModeJoints.size(); Link* root = body->rootLink(); if(given_rootDof){ pGiven = root->p; RGiven = root->R; voGiven = root->vo; wGiven = root->w; root->p = pGivenPrev; root->R = RGivenPrev; root->vo = voGivenPrev; root->w = wGivenPrev; } for(int i=0; i < numHighGainJoints; ++i){ Link* link = highGainModeJoints[i]; qGiven [i] = link->q; dqGiven[i] = link->dq; link->q = qGivenPrev[i]; link->dq = dqGivenPrev[i]; } if(unknown_rootDof || given_rootDof){ p0 = root->p; R0 = root->R; vo0 = root->vo; w0 = root->w; } vo.setZero(); w.setZero(); dvo.setZero(); dw.setZero(); int numLinks = body->numLinks(); for(int i=1; i < numLinks; ++i){ Link* link = body->link(i); q0 [i] = link->q; dq0[i] = link->dq; dq [i] = 0.0; ddq[i] = 0.0; } sumExternalForces(); solveUnknownAccels(); calcAccelFKandForceSensorValues(); integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0); calcPositionAndVelocityFK(); calcMassMatrix(); solveUnknownAccels(); integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0); calcPositionAndVelocityFK(); calcMassMatrix(); solveUnknownAccels(); integrateRungeKuttaOneStep(2.0 / 6.0, timeStep); calcPositionAndVelocityFK(); calcMassMatrix(); solveUnknownAccels(); if(unknown_rootDof){ SE3exp(root->p, root->R, p0, R0, w0, vo0, timeStep); root->vo = vo0 + (dvo + root->dvo / 6.0) * timeStep; root->w = w0 + (dw + root->dw / 6.0) * timeStep; } if(given_rootDof){ root->p = pGiven; root->R = RGiven; root->vo = voGiven; root->w = wGiven; } for(size_t i=0; i < torqueModeJoints.size(); ++i){ Link* link = torqueModeJoints[i]; int index = link->index; link->q = q0 [index] + (dq [index] + link->dq / 6.0) * timeStep; link->dq = dq0[index] + (ddq[index] + link->ddq / 6.0) * timeStep; } for(size_t i=0; i < highGainModeJoints.size(); ++i){ Link* link = highGainModeJoints[i]; link->q = qGiven [i]; link->dq = dqGiven[i]; } calcPositionAndVelocityFK(); calcMassMatrix(); preserveHighGainModeJointState(); } void ForwardDynamicsMM::integrateRungeKuttaOneStep(double r, double dt) { Link* root = body->rootLink(); if(unknown_rootDof || given_rootDof){ SE3exp(root->p, root->R, p0, R0, root->w, root->vo, dt); root->vo = vo0 + root->dvo * dt; root->w = w0 + root->dw * dt; vo += r * root->vo; w += r * root->w; dvo += r * root->dvo; dw += r * root->dw; } int n = body->numLinks(); for(int i=1; i < n; ++i){ Link* link = body->link(i); link->q = q0 [i] + dt * link->dq; link->dq = dq0[i] + dt * link->ddq; dq [i] += r * link->dq; ddq[i] += r * link->ddq; } } void ForwardDynamicsMM::preserveHighGainModeJointState() { if(given_rootDof){ Link* root = body->rootLink(); pGivenPrev = root->p; RGivenPrev = root->R; voGivenPrev = root->vo; wGivenPrev = root->w; } for(size_t i=0; i < highGainModeJoints.size(); ++i){ Link* link = highGainModeJoints[i]; qGivenPrev [i] = link->q; dqGivenPrev[i] = link->dq; } } void ForwardDynamicsMM::calcPositionAndVelocityFK() { const LinkTraverse& traverse = body->linkTraverse(); int n = traverse.numLinks(); Link* root = traverse[0]; root_w_x_v = root->w.cross(root->vo + root->w.cross(root->p)); if(given_rootDof){ root->vo = root->v - root->w.cross(root->p); } for(int i=0; i < n; ++i){ Link* link = traverse[i]; Link* parent = link->parent; if(parent){ switch(link->jointType){ case Link::SLIDE_JOINT: link->p.noalias() = parent->R * (link->b + link->q * link->d) + parent->p; link->R = parent->R; link->sw.setZero(); link->sv.noalias() = parent->R * link->d; link->w = parent->w; break; case Link::ROTATIONAL_JOINT: link->R.noalias() = parent->R * AngleAxisd(link->q, link->a); link->p.noalias() = parent->R * link->b + parent->p; link->sw.noalias() = parent->R * link->a; link->sv = link->p.cross(link->sw); link->w = link->dq * link->sw + parent->w; break; case Link::FIXED_JOINT: default: link->p.noalias() = parent->R * link->b + parent->p; link->R = parent->R; link->w = parent->w; link->vo = parent->vo; link->sw.setZero(); link->sv.setZero(); link->cv.setZero(); link->cw.setZero(); goto COMMON_CALCS_FOR_ALL_JOINT_TYPES; } link->vo = link->dq * link->sv + parent->vo; const Vector3 dsv = parent->w.cross(link->sv) + parent->vo.cross(link->sw); const Vector3 dsw = parent->w.cross(link->sw); link->cv = link->dq * dsv; link->cw = link->dq * dsw; } COMMON_CALCS_FOR_ALL_JOINT_TYPES: /// \todo remove this equation link->v = link->vo + link->w.cross(link->p); link->wc.noalias() = link->R * link->c + link->p; const Matrix3 Iw = link->R * link->I * link->R.transpose(); const Matrix3 c_hat = hat(link->wc); link->Iww.noalias() = link->m * c_hat * c_hat.transpose() + Iw; link->Iwv = link->m * c_hat; const Vector3 P = link->m * (link->vo + link->w.cross(link->wc)); const Vector3 L = link->Iww * link->w + link->m * link->wc.cross(link->vo); link->pf = link->w.cross(P); link->ptau = link->vo.cross(P) + link->w.cross(L); } } /** calculate the mass matrix using the unit vector method \todo replace the unit vector method here with a more efficient method that only requires O(n) computation time */ void ForwardDynamicsMM::calcMassMatrix() { Link* root = body->rootLink(); int numLinks = body->numLinks(); // preserve and clear the joint accelerations for(int i=1; i < numLinks; ++i){ Link* link = body->link(i); ddqorg[i] = link->ddq; uorg [i] = link->u; link->ddq = 0.0; } // preserve and clear the root link acceleration dvoorg = root->dvo; dworg = root->dw; root->dvo = g - root_w_x_v; // dv = g, dw = 0 root->dw.setZero(); setColumnOfMassMatrix(b1, 0); if(unknown_rootDof){ for(int i=0; i < 3; ++i){ root->dvo[i] += 1.0; setColumnOfMassMatrix(M11, i); root->dvo[i] -= 1.0; } for(int i=0; i < 3; ++i){ root->dw[i] = 1.0; Vector3 dw_x_p = root->dw.cross(root->p); root->dvo -= dw_x_p; setColumnOfMassMatrix(M11, i + 3); root->dvo += dw_x_p; root->dw[i] = 0.0; } } if(given_rootDof){ for(int i=0; i < 3; ++i){ root->dvo[i] += 1.0; setColumnOfMassMatrix(M12, i); root->dvo[i] -= 1.0; } for(int i=0; i < 3; ++i){ root->dw[i] = 1.0; Vector3 dw_x_p = root->dw.cross(root->p); root->dvo -= dw_x_p; setColumnOfMassMatrix(M12, i + 3); root->dvo += dw_x_p; root->dw[i] = 0.0; } } for(size_t i=0; i < torqueModeJoints.size(); ++i){ Link* link = torqueModeJoints[i]; link->ddq = 1.0; int j = i + unknown_rootDof; setColumnOfMassMatrix(M11, j); M11(j, j) += link->Jm2; // motor inertia link->ddq = 0.0; } for(size_t i=0; i < highGainModeJoints.size(); ++i){ Link* link = highGainModeJoints[i]; link->ddq = 1.0; int j = i + given_rootDof; setColumnOfMassMatrix(M12, j); link->ddq = 0.0; } // subtract the constant term for(int i=0; i < M11.cols(); ++i){ M11.col(i) -= b1; } for(int i=0; i < M12.cols(); ++i){ M12.col(i) -= b1; } for(int i=1; i < numLinks; ++i){ Link* link = body->link(i); link->ddq = ddqorg[i]; link->u = uorg [i]; } root->dvo = dvoorg; root->dw = dworg; accelSolverInitialized = false; } void ForwardDynamicsMM::setColumnOfMassMatrix(MatrixXd& M, int column) { Vector3 f; Vector3 tau; Link* root = body->rootLink(); calcInverseDynamics(root, f, tau); MatrixXd::ColXpr col = M.col(column); if(unknown_rootDof){ tau -= root->p.cross(f); col.head(3) = f; col.segment(3, 3) = tau; } for(size_t i=0; i < torqueModeJoints.size(); ++i){ col(i + unknown_rootDof) = torqueModeJoints[i]->u; } } void ForwardDynamicsMM::calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau) { Link* parent = link->parent; if(parent){ link->dvo = parent->dvo + link->cv + link->sv * link->ddq; link->dw = parent->dw + link->cw + link->sw * link->ddq; } out_f = link->pf; out_tau = link->ptau; if(link->child){ Vector3 f_c; Vector3 tau_c; calcInverseDynamics(link->child, f_c, tau_c); out_f += f_c; out_tau += tau_c; } out_f .noalias() += link->m * link->dvo + link->Iwv.transpose() * link->dw; out_tau.noalias() += link->Iwv * link->dvo + link->Iww * link->dw; link->u = link->sv.dot(out_f) + link->sw.dot(out_tau); if(link->sibling){ Vector3 f_s; Vector3 tau_s; calcInverseDynamics(link->sibling, f_s, tau_s); out_f += f_s; out_tau += tau_s; } } void ForwardDynamicsMM::sumExternalForces() { fextTotal.setZero(); tauextTotal.setZero(); int n = body->numLinks(); for(int i=0; i < n; ++i){ Link* link = body->link(i); fextTotal += link->fext; tauextTotal += link->tauext; } tauextTotal -= body->rootLink()->p.cross(fextTotal); } void ForwardDynamicsMM::calcd1(Link* link, Vector3& out_f, Vector3& out_tau) { out_f = -link->fext; out_tau = -link->tauext; if(link->child){ Vector3 f_c; Vector3 tau_c; calcd1(link->child, f_c, tau_c); out_f += f_c; out_tau += tau_c; } link->u = link->sv.dot(out_f) + link->sw.dot(out_tau); if(link->sibling){ Vector3 f_s; Vector3 tau_s; calcd1(link->sibling, f_s, tau_s); out_f += f_s; out_tau += tau_s; } } void ForwardDynamicsMM::initializeAccelSolver() { if(!accelSolverInitialized){ if(!ddqGivenCopied){ if(given_rootDof){ Link* root = body->rootLink(); root->dvo = root->dv - root->dw.cross(root->p) - root->w.cross(root->v); ddqGiven.head(3) = root->dvo; ddqGiven.segment(3, 3) = root->dw; } for(size_t i=0; i < highGainModeJoints.size(); ++i){ ddqGiven(given_rootDof + i) = highGainModeJoints[i]->ddq; } ddqGivenCopied = true; } b1 += M12 * ddqGiven; for(int i=1; i < body->numLinks(); ++i){ Link* link = body->link(i); uorg [i] = link->u; } Vector3 f, tau; Link* root = body->rootLink(); calcd1(root, f, tau); for(int i=0; i < unknown_rootDof; i++){ d1(i, 0) = 0; } for(size_t i=0; i < torqueModeJoints.size(); ++i){ d1(i + unknown_rootDof, 0) = torqueModeJoints[i]->u; } for(int i=1; i < body->numLinks(); ++i){ Link* link = body->link(i); link->u = uorg [i]; } accelSolverInitialized = true; } } //for ConstraintForceSolver void ForwardDynamicsMM::solveUnknownAccels (Link* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext) { const Vector3 fextorg = link->fext; const Vector3 tauextorg = link->tauext; link->fext = fext; link->tauext = tauext; for(int i=1; i < body->numLinks(); ++i){ Link* link = body->link(i); uorg [i] = link->u; } Vector3 f, tau; Link* root = body->rootLink(); calcd1(root, f, tau); for(int i=0; iu; } for(int i=1; i < body->numLinks(); ++i){ Link* link = body->link(i); link->u = uorg [i]; } link->fext = fextorg; link->tauext = tauextorg; solveUnknownAccels(rootfext,roottauext); } void ForwardDynamicsMM::solveUnknownAccels(const Vector3& fext, const Vector3& tauext) { if(unknown_rootDof){ c1.head(3) = fext; c1.segment(3, 3) = tauext; } for(size_t i=0; i < torqueModeJoints.size(); ++i){ c1(i + unknown_rootDof) = torqueModeJoints[i]->u; } c1 -= d1; c1 -= b1.col(0); VectorXd a(M11.colPivHouseholderQr().solve(c1)); if(unknown_rootDof){ Link* root = body->rootLink(); root->dw = a.segment(3, 3); const Vector3 dv = a.head(3); root->dvo = dv - root->dw.cross(root->p) - root_w_x_v; } for(size_t i=0; i < torqueModeJoints.size(); ++i){ Link* link = torqueModeJoints[i]; link->ddq = a(i + unknown_rootDof); } } void ForwardDynamicsMM::calcAccelFKandForceSensorValues(Link* link, Vector3& out_f, Vector3& out_tau) { Link* parent = link->parent; if(parent){ link->dvo = parent->dvo + link->cv + link->sv * link->ddq; link->dw = parent->dw + link->cw + link->sw * link->ddq; } out_f = link->pf; out_tau = link->ptau; for(Link* child = link->child; child; child = child->sibling){ Vector3 f, tau; calcAccelFKandForceSensorValues(child, f, tau); out_f += f; out_tau += tau; } ForceSensorInfo& info = forceSensorInfo[link->index]; if(CALC_ALL_JOINT_TORQUES || info.hasSensorsAbove){ Vector3 fg(-link->m * g); Vector3 tg(link->wc.cross(fg)); out_f -= fg; out_tau -= tg; out_f -= link->fext; out_tau -= link->tauext; out_f .noalias() += link->m * link->dvo + link->Iwv.transpose() * link->dw; out_tau.noalias() += link->Iwv * link->dvo + link->Iww * link->dw; if(CALC_ALL_JOINT_TORQUES && link->isHighGainMode){ link->u = link->sv.dot(out_f) + link->sw.dot(out_tau); } if(info.sensor){ ForceSensor* sensor = info.sensor; const Matrix3 sensorR = link->R * sensor->localR; const Vector3 sensorPos = link->p + link->R * sensor->localPos; const Vector3 f = -out_f; sensor->f = sensorR.transpose() * f; sensor->tau = sensorR.transpose() * (-out_tau - sensorPos.cross(f)); } } } void ForwardDynamicsMM::initializeSensors() { ForwardDynamics::initializeSensors(); int n = body->numLinks(); forceSensorInfo.resize(n); for(int i=0; i < n; ++i){ forceSensorInfo[i].sensor = 0; forceSensorInfo[i].hasSensorsAbove = false; } if(sensorsEnabled){ int numForceSensors = body->numSensors(Sensor::FORCE); for(int i=0; i < numForceSensors; ++i){ ForceSensor* sensor = body->sensor(i); forceSensorInfo[sensor->link->index].sensor = sensor; } updateForceSensorInfo(body->rootLink(), false); } } void ForwardDynamicsMM::updateForceSensorInfo(Link* link, bool hasSensorsAbove) { ForceSensorInfo& info = forceSensorInfo[link->index]; hasSensorsAbove |= (info.sensor != 0); info.hasSensorsAbove = hasSensorsAbove; for(Link* child = link->child; child; child = child->sibling){ updateForceSensorInfo(child, hasSensorsAbove); } } choreonoid-1.1.0+dfsg/src/Body/ForwardDynamicsCBM.h000066400000000000000000000103231207742442300220400ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_FORWARD_DYNAMICS_MM_H_INCLUDED #define CNOID_BODY_FORWARD_DYNAMICS_MM_H_INCLUDED #include "ForwardDynamics.h" #include #include #include #include "exportdecl.h" namespace cnoid { class Link; class Body; typedef boost::intrusive_ptr BodyPtr; class LinkTraverse; class AccelSensor; class ForceSensor; /** The ForwardDynamicsMM class calculates the forward dynamics using the motion equation based on the generalized mass matrix. The class is mainly used for a body that has high-gain mode joints. If all the joints of a body are the torque mode, the ForwardDynamicsABM, which uses the articulated body method, is more efficient. */ class CNOID_EXPORT ForwardDynamicsMM : public ForwardDynamics { public: ForwardDynamicsMM(BodyPtr body); ~ForwardDynamicsMM(); virtual void initialize(); virtual void calcNextState(); void initializeAccelSolver(); void solveUnknownAccels(const Vector3& fext, const Vector3& tauext); void solveUnknownAccels(Link* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext); private: /* Elements of the motion equation | | | | dv | | b1 | | 0 | | totalfext | | M11 | M12 | | dw | | | | 0 | | totaltauext | | | | * |ddq (unkown)| + | | + | d1 | = | u (known) | |-----+-----| |------------| |----| |----| |----------------| | M21 | M22 | | given ddq | | b2 | | d2 | | u2 | |fext | d1 = trans(s)| | |tauext| */ MatrixXd M11; MatrixXd M12; MatrixXd b1; VectorXd d1; VectorXd c1; std::vector torqueModeJoints; std::vector highGainModeJoints; //int rootDof; // dof of dv and dw (0 or 6) int unknown_rootDof; int given_rootDof; bool isNoUnknownAccelMode; VectorXd qGiven; VectorXd dqGiven; VectorXd ddqGiven; Vector3 pGiven; Matrix3 RGiven; Vector3 voGiven; Vector3 wGiven; bool accelSolverInitialized; bool ddqGivenCopied; VectorXd qGivenPrev; VectorXd dqGivenPrev; Vector3 pGivenPrev; Matrix3 RGivenPrev; Vector3 voGivenPrev; Vector3 wGivenPrev; Vector3 fextTotal; Vector3 tauextTotal; Vector3 root_w_x_v; // buffers for the unit vector method VectorXd ddqorg; VectorXd uorg; Vector3 dvoorg; Vector3 dworg; struct ForceSensorInfo { ForceSensor* sensor; bool hasSensorsAbove; }; std::vector forceSensorInfo; // Buffers for the Runge Kutta Method Vector3 p0; Matrix3 R0; Vector3 vo0; Vector3 w0; std::vector q0; std::vector dq0; Vector3 vo; Vector3 w; Vector3 dvo; Vector3 dw; std::vector dq; std::vector ddq; virtual void initializeSensors(); void calcMotionWithEulerMethod(); void calcMotionWithRungeKuttaMethod(); void integrateRungeKuttaOneStep(double r, double dt); void preserveHighGainModeJointState(); void calcPositionAndVelocityFK(); void calcMassMatrix(); void setColumnOfMassMatrix(MatrixXd& M, int column); void calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau); void calcd1(Link* link, Vector3& out_f, Vector3& out_tau); void sumExternalForces(); inline void solveUnknownAccels(); inline void calcAccelFKandForceSensorValues(); void calcAccelFKandForceSensorValues(Link* link, Vector3& out_f, Vector3& out_tau); void updateForceSensorInfo(Link* link, bool hasSensorsAbove); }; typedef boost::shared_ptr ForwardDynamicsMMPtr; }; #endif choreonoid-1.1.0+dfsg/src/Body/InverseDynamics.cpp000066400000000000000000000036671207742442300220750ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "InverseDynamics.h" #include "Link.h" #include /* * see Kajita et al. Humanoid Robot Ohm-sha, p.210 */ using namespace cnoid; namespace cnoid { void calcInverseDynamics(const BodyPtr& body, Link* ptr, Vector3& out_f, Vector3& out_tau) { Link* parent = ptr->parent; if(parent){ Vector3 sv, sw; if(ptr->jointType != Link::FIXED_JOINT){ sw.noalias() = parent->R * ptr->a; sv = ptr->p.cross(sw); } else { sw.setZero(); sv.setZero(); } const Vector3 dsv = parent->w.cross(sv) + parent->vo.cross(sw); const Vector3 dsw = parent->w.cross(sw); ptr->dw = parent->dw + dsw * ptr->dq + sw * ptr->ddq; ptr->dvo = parent->dvo + dsv * ptr->dq + sv * ptr->ddq; ptr->sw = sw; ptr->sv = sv; } const Vector3 c = ptr->R * ptr->c + ptr->p; Matrix3 I = ptr->R * ptr->I * ptr->R.transpose(); const Matrix3 c_hat = hat(c); I.noalias() += ptr->m * c_hat * c_hat.transpose(); const Vector3 P = ptr->m * (ptr->vo + ptr->w.cross(c)); const Vector3 L = ptr->m * c.cross(ptr->vo) + I * ptr->w; out_f = ptr->m * (ptr->dvo + ptr->dw.cross(c)) + ptr->w.cross(P); out_tau = ptr->m * c.cross(ptr->dvo) + I * ptr->dw + ptr->vo.cross(P) + ptr->w.cross(L); if(ptr->child){ Vector3 f_c; Vector3 tau_c; calcInverseDynamics(body, ptr->child, f_c, tau_c); out_f += f_c; out_tau += tau_c; } ptr->u = ptr->sv.dot(out_f) + ptr->sw.dot(out_tau); if(ptr->sibling){ Vector3 f_s; Vector3 tau_s; calcInverseDynamics(body, ptr->sibling, f_s, tau_s); out_f += f_s; out_tau += tau_s; } } } choreonoid-1.1.0+dfsg/src/Body/InverseDynamics.h000066400000000000000000000004751207742442300215340ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_INVERSE_DYNAMICS_H_INCLUDED #define CNOID_BODY_INVERSE_DYNAMICS_H_INCLUDED #include "Body.h" #include "exportdecl.h" namespace cnoid { CNOID_EXPORT void calcInverseDynamics(const BodyPtr& body, Link* link, Vector3& out_f, Vector3& out_tau); } #endif choreonoid-1.1.0+dfsg/src/Body/InverseKinematics.h000066400000000000000000000012061207742442300220450ustar00rootroot00000000000000/** \author Shin'ichiro Nakaoka */ #ifndef CNOID_INVERSE_KINEMATICS_H_INCLUDED #define CNOID_INVERSE_KINEMATICS_H_INCLUDED #include #include namespace cnoid { class InverseKinematics { public: enum AxisSet { NO_AXES = 0, TRANSLATION_3D = 0x1, ROTATION_3D = 0x2, TRANSFORM_6D = 0x3 }; virtual ~InverseKinematics() { } virtual AxisSet axisType() const { return TRANSFORM_6D; } virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R) = 0; }; typedef boost::shared_ptr InverseKinematicsPtr; } #endif choreonoid-1.1.0+dfsg/src/Body/Jacobian.cpp000066400000000000000000000055741207742442300204770ustar00rootroot00000000000000 #include "Jacobian.h" #include "Link.h" #include "JointPath.h" #include namespace cnoid { /** @brief compute CoM Jacobian @param base link fixed to the environment @param J CoM Jacobian @note Link::wc must be computed by calcCM() before calling */ void calcCMJacobian(const BodyPtr& body, Link* base, Eigen::MatrixXd& J) { // prepare subm, submwc const int nj = body->numJoints(); Link* rootLink = body->rootLink(); JointPathPtr jp; if(base){ jp = body->getJointPath(rootLink, base); Link* skip = jp->joint(0); skip->subm = rootLink->m; skip->submwc = rootLink->m * rootLink->wc; Link* l = rootLink->child; if(l){ if(l != skip){ l->calcSubMassCM(); skip->subm += l->subm; skip->submwc += l->submwc; } l = l->sibling; while(l){ if (l != skip){ l->calcSubMassCM(); skip->subm += l->subm; skip->submwc += l->submwc; } l = l->sibling; } } // assuming there is no branch between base and root for(int i=1; i < jp->numJoints(); i++){ l = jp->joint(i); l->subm = l->parent->m + l->parent->subm; l->submwc = l->parent->m * l->parent->wc + l->parent->submwc; } J.resize(3, nj); }else{ rootLink->calcSubMassCM(); J.resize(3, nj + 6); } // compute Jacobian std::vector sgn(nj, 1); if(jp){ for(int i=0; i < jp->numJoints(); i++){ sgn[jp->joint(i)->jointId] = -1; } } for(int i=0; i < nj; i++){ Link* j = body->joint(i); switch(j->jointType){ case Link::ROTATIONAL_JOINT: { const Vector3 omega = sgn[j->jointId] * j->R * j->a; const Vector3 arm = (j->submwc - j->subm * j->p) / body->totalMass(); const Vector3 dp = omega.cross(arm); J.col(j->jointId) = dp; break; } default: std::cerr << "calcCMJacobian() : unsupported jointType(" << j->jointType << std::endl; } } if(!base){ const int c = nj; J.block(0, c, 3, 3).setIdentity(); const Vector3 dp = rootLink->submwc / body->totalMass() - rootLink->p; J.block(0, c + 3, 3, 3) << 0.0, dp(2), -dp(1), -dp(2), 0.0, dp(0), dp(1), -dp(0), 0.0; } } } choreonoid-1.1.0+dfsg/src/Body/Jacobian.h000066400000000000000000000003701207742442300201310ustar00rootroot00000000000000 #ifndef CNOID_BODY_JACOBIAN_H_INCLUDED #define CNOID_BODY_JACOBIAN_H_INCLUDED #include "Body.h" #include "exportdecl.h" namespace cnoid { CNOID_EXPORT void calcCmJacobian(const BodyPtr& body, Link* base, Eigen::MatrixXd& J); } #endif choreonoid-1.1.0+dfsg/src/Body/JointPath.cpp000066400000000000000000000140001207742442300206510ustar00rootroot00000000000000/** \file \brief Implementations of the LinkPath class \author Shin'ichiro Nakaoka */ #include "JointPath.h" #include "Link.h" #include #include using namespace std; using namespace cnoid; JointPath::JointPath() { initialize(); } JointPath::JointPath(Link* base, Link* end) : linkPath(base, end), joints(linkPath.size()) { initialize(); extractJoints(); } JointPath::JointPath(Link* end) : linkPath(end), joints(linkPath.size()) { initialize(); extractJoints(); } void JointPath::initialize() { maxIKerrorSqr = 1.0e-6 * 1.0e-6; isBestEffortIKMode = false; } JointPath::~JointPath() { } bool JointPath::find(Link* base, Link* end) { if(linkPath.find(base, end)){ extractJoints(); } onJointPathUpdated(); return (!joints.empty()); } bool JointPath::find(Link* end) { linkPath.find(end); extractJoints(); onJointPathUpdated(); return !joints.empty(); } void JointPath::extractJoints() { numUpwardJointConnections = 0; int n = linkPath.size(); if(n <= 1){ joints.clear(); } else { int i = 0; if(linkPath.isDownward(i)){ i++; } joints.resize(n); // reserve size n buffer joints.clear(); int m = n - 1; while(i < m){ Link* link = linkPath[i]; if(link->jointId >= 0){ if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){ joints.push_back(link); if(!linkPath.isDownward(i)){ numUpwardJointConnections++; } } } ++i; } if(linkPath.isDownward(m-1)){ Link* link = linkPath[m]; if(link->jointId >= 0){ if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){ joints.push_back(link); } } } } } void JointPath::onJointPathUpdated() { } void JointPath::calcJacobian(Eigen::MatrixXd& out_J) const { const int n = joints.size(); out_J.resize(6, n); if(n > 0){ Link* targetLink = linkPath.endLink(); for(int i=0; i < n; ++i){ Link* link = joints[i]; switch(link->jointType){ case Link::ROTATIONAL_JOINT: { Vector3 omega = link->R * link->a; const Vector3 arm = targetLink->p - link->p; if(!isJointDownward(i)){ omega = -omega; } out_J.col(i) << omega.cross(arm), omega; } break; case Link::SLIDE_JOINT: { Vector3 dp = link->R * link->d; if(!isJointDownward(i)){ dp = -dp; } out_J.col(i) << dp, Vector3::Zero(); } break; default: out_J.col(i).setZero(); } } } } void JointPath::setMaxIKerror(double e) { maxIKerrorSqr = e * e; } void JointPath::setBestEffortIKMode(bool on) { isBestEffortIKMode = on; } bool JointPath::calcInverseKinematics (const Vector3& base_p, const Matrix3& base_R, const Vector3& end_p, const Matrix3& end_R) { Link* baseLink = linkPath.baseLink(); baseLink->p = base_p; baseLink->R = base_R; if(!hasAnalyticalIK()){ calcForwardKinematics(); } return calcInverseKinematics(end_p, end_R); } bool JointPath::calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R) { static const int MAX_IK_ITERATION = 50; static const double LAMBDA = 0.9; if(joints.empty()){ if(linkPath.empty()){ return false; } if(baseLink() == endLink()){ baseLink()->p = end_p; baseLink()->R = end_R; return true; } else { // \todo implement here return false; } } const int n = numJoints(); Link* target = linkPath.endLink(); std::vector qorg(n); for(int i=0; i < n; ++i){ qorg[i] = joints[i]->q; } MatrixXd J(6, n); VectorXd dq(n); double errsqr = std::numeric_limits::max(); bool converged = false; for(int i=0; i < MAX_IK_ITERATION; i++){ const Vector3 dp = end_p - target->p; const Vector3 omegaLocal = omegaFromRot(target->R.transpose() * end_R); if(isBestEffortIKMode){ const double errsqr0 = errsqr; errsqr = dp.squaredNorm() + omegaLocal.squaredNorm(); if(fabs(errsqr - errsqr0) < maxIKerrorSqr){ converged = true; break; } } else { errsqr = dp.squaredNorm() + omegaLocal.squaredNorm(); if(errsqr < maxIKerrorSqr){ converged = true; break; } } calcJacobian(J); Eigen::Matrix v; v << dp, (target->R * omegaLocal); if(n == 6){ dq = J.colPivHouseholderQr().solve(v); } else { //dq = J.fullPivHouseholderQr().solve(v); dq = Eigen::JacobiSVD(J, Eigen::ComputeThinU | Eigen::ComputeThinV).solve(v); } for(int j=0; j < n; ++j){ joints[j]->q += LAMBDA * dq(j); } calcForwardKinematics(); } if(!converged){ for(int i=0; i < n; ++i){ joints[i]->q = qorg[i]; } calcForwardKinematics(); } return converged; } bool JointPath::hasAnalyticalIK() const { return false; } std::ostream& operator<<(std::ostream& os, JointPath& path) { int n = path.numJoints(); for(int i=0; i < n; ++i){ Link* link = path.joint(i); os << link->name(); if(i != n){ os << (path.isJointDownward(i) ? " => " : " <= "); } } os << std::endl; return os; } choreonoid-1.1.0+dfsg/src/Body/JointPath.h000066400000000000000000000043171207742442300203300ustar00rootroot00000000000000/** \file \brief The header file of the LinkPath and JointPath classes \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_JOINT_PATH_H_INCLUDED #define CNOID_BODY_JOINT_PATH_H_INCLUDED #include "LinkPath.h" #include "InverseKinematics.h" #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT JointPath : public InverseKinematics { public: JointPath(); JointPath(Link* base, Link* end); JointPath(Link* end); virtual ~JointPath(); bool find(Link* base, Link* end); bool find(Link* end); inline bool empty() const { return joints.empty(); } inline int numJoints() const { return joints.size(); } inline Link* joint(int index) const { return joints[index]; } inline Link* baseLink() const { return linkPath.baseLink(); } inline Link* endLink() const { return linkPath.endLink(); } inline bool isJointDownward(int index) const { return (index >= numUpwardJointConnections); } inline void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false) const { linkPath.calcForwardKinematics(calcVelocity, calcAcceleration); } void calcJacobian(Eigen::MatrixXd& out_J) const; void setMaxIKerror(double e); void setBestEffortIKMode(bool on); // InverseKinematics Interface virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R); virtual bool hasAnalyticalIK() const; bool calcInverseKinematics( const Vector3& base_p, const Matrix3& base_R, const Vector3& end_p, const Matrix3& end_R); protected: virtual void onJointPathUpdated(); double maxIKerrorSqr; bool isBestEffortIKMode; private: void initialize(); void extractJoints(); LinkPath linkPath; std::vector joints; int numUpwardJointConnections; }; typedef boost::shared_ptr JointPathPtr; }; #endif choreonoid-1.1.0+dfsg/src/Body/LeggedBody.cpp000066400000000000000000000125741207742442300207740ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "LeggedBody.h" #include "Link.h" #include "JointPath.h" #include #include using namespace cnoid; LeggedBody::LeggedBody() { } LeggedBody::LeggedBody(const LeggedBody& org) : Body(org) { } LeggedBody::~LeggedBody() { } BodyPtr LeggedBody::duplicate() const { return new LeggedBody(*this); } bool LeggedBody::checkBodyInfoAsLeggedBody(const YamlMappingPtr info) { return info->findSequence("footLinks")->isValid(); } void LeggedBody::doResetInfo(const YamlMapping& info) { Body::doResetInfo(info); footInfos.clear(); const YamlSequence& footLinkNodes = *info["footLinks"].toSequence(); for(int i=0; i < footLinkNodes.size(); ++i){ FootInfo footInfo; footInfo.info = footLinkNodes[i].toMapping(); const YamlMapping& footLinkNode = *footInfo.info; footInfo.link = link(footLinkNode["link"].toString()); if(footInfo.link){ readEx(footLinkNode, "soleCenter", footInfo.soleCenter); if(!read(footLinkNode, "homeCop", footInfo.homeCop)){ footInfo.homeCop = footInfo.soleCenter; } footInfos.push_back(footInfo); } } } bool LeggedBody::doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor) { if(footInfos.empty()){ return false; } static const int MAX_ITERATION = 100; static const double ERROR_THRESH_SQR = 1.0e-6 * 1.0e-6; Link* baseFoot = footInfos[0].link; Link* waist = rootLink(); LinkTraverse fkTraverse(waist); Vector3 c0 = calcCM(); bool failed = false; int loopCounter = 0; while(true){ Vector3 e = c - c0; if(onlyProjectionToFloor){ e.z() = 0.0; } if(e.squaredNorm() < ERROR_THRESH_SQR){ break; } size_t numDone = 0; JointPathPtr baseToWaist = getJointPath(baseFoot, waist); if(baseToWaist && baseToWaist->calcInverseKinematics(waist->p + e, waist->R)){ numDone++; for(size_t j=1; j < footInfos.size(); ++j){ Link* foot = footInfos[j].link; JointPathPtr waistToFoot = getJointPath(waist, foot); if(waistToFoot){ bool ikDone; if(waistToFoot->hasAnalyticalIK()){ ikDone = waistToFoot->calcInverseKinematics(foot->p, foot->R); } else { Vector3 p0 = foot->p; Matrix3 R0 = foot->R; waistToFoot->calcForwardKinematics(); ikDone = waistToFoot->calcInverseKinematics(p0, R0); } if(ikDone){ numDone++; } else { break; } } } } if(numDone < footInfos.size()){ failed = true; break; } if(++loopCounter < MAX_ITERATION){ fkTraverse.calcForwardKinematics(); c0 = calcCM(); } else { break; } } return !failed; } bool LeggedBody::setStance(double width, Link* baseLink) { if(footInfos.size() != 2){ return false; } bool result = false; Link* foot[2]; double sign; if(footInfos[1].link == baseLink){ foot[0] = footInfos[1].link; foot[1] = footInfos[0].link; sign = -1.0; } else { foot[0] = footInfos[0].link; foot[1] = footInfos[1].link; sign = 1.0; } const Matrix3& R0 = foot[0]->R; const Vector3 baseY(R0(0,1), sign * R0(1,1), 0.0); Link* waist = rootLink(); foot[1]->p = foot[0]->p + baseY * width; Vector3 wp = (foot[0]->p + foot[1]->p) / 2.0; wp[2] = waist->p[2]; JointPathPtr ikPath = getJointPath(foot[0], waist); if(ikPath && ikPath->calcInverseKinematics(wp, waist->R)){ ikPath = getJointPath(waist, foot[1]); if(ikPath && ikPath->calcInverseKinematics(foot[1]->p, foot[1]->R)){ LinkTraverse fkTraverse(baseLink); fkTraverse.calcForwardKinematics(); result = true; } } return result; } Vector3 LeggedBody::centerOfSole(int footIndex) const { const FootInfo& info = footInfos[footIndex]; return info.link->p + info.link->R * info.soleCenter; } Vector3 LeggedBody::centerOfSoles() const { Vector3 p = Vector3::Zero(); int n = footInfos.size(); if(n == 0){ throw "LeggedBody::centerOfSoles(): not foot information"; } else { for(int i=0; i < n; ++i){ const FootInfo& info = footInfos[i]; p.noalias() += info.link->p + info.link->R * info.soleCenter; } } return p / n; } Vector3 LeggedBody::homeCopOfSole(int footIndex) const { const FootInfo& info = footInfos[footIndex]; return info.link->p + info.link->R * info.homeCop; } Vector3 LeggedBody::homeCopOfSoles() const { Vector3 p = Vector3::Zero(); int n = footInfos.size(); if(n == 0){ throw "LeggedBody::homeCopOfSoles(): not foot information"; } else { for(size_t i=0; i < footInfos.size(); ++i){ const FootInfo& info = footInfos[i]; p.noalias() += info.link->p + info.link->R * info.homeCop; } } return p / n; } choreonoid-1.1.0+dfsg/src/Body/LeggedBody.h000066400000000000000000000026271207742442300204370ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_LEGGED_BODY_H_INCLUDED #define CNOID_BODY_LEGGED_BODY_H_INCLUDED #include "Body.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT LeggedBody : public Body { public: LeggedBody(); virtual ~LeggedBody(); virtual BodyPtr duplicate() const; inline int numFeet() const { return footInfos.size(); } struct FootInfo { Link* link; Vector3 homeCop; Vector3 soleCenter; YamlMappingPtr info; }; inline Link* footLink(int index) const { return footInfos[index].link; } inline const FootInfo& footInfo(int index) const { return footInfos[index]; } bool doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor = false); bool setStance(double width, Link* baseLink); Vector3 centerOfSole(int footIndex) const; Vector3 centerOfSoles() const; Vector3 homeCopOfSole(int footIndex) const; Vector3 homeCopOfSoles() const; static bool checkBodyInfoAsLeggedBody(const YamlMappingPtr info); protected: LeggedBody(const LeggedBody& org); virtual void doResetInfo(const YamlMapping& info); private: std::vector footInfos; }; typedef boost::intrusive_ptr LeggedBodyPtr; } #endif choreonoid-1.1.0+dfsg/src/Body/Link.cpp000066400000000000000000000151161207742442300176570ustar00rootroot00000000000000/** \file \brief Implementations of Link class \author Shin'ichiro Nakaoka */ /** \ifnot jp \class OpenHRP::Link A class for representing a rigid body that consists of an articulated body. \var int Link::index Index among all the links in the body. \endif */ /** \if jp \class OpenHRP::Link 多関節モデルã®ä¸­ã®å€‹ã€…ã®å‰›ä½“（リンク）を表ã™ã‚¯ãƒ©ã‚¹ã€‚ \var int Link::index 全リンクを対象ã¨ã—ãŸãƒªãƒ³ã‚¯ã®ã‚¤ãƒ³ãƒ‡ãƒƒã‚¯ã‚¹ã€‚ モデルファイルã«ãŠã‘ã‚‹Jointノード定義ã®å‡ºç¾é †ï¼ˆãƒ«ãƒ¼ãƒˆã‹ã‚‰ã®æŽ¢ç´¢é †ï¼‰ã«å¯¾å¿œã™ã‚‹ã€‚ ãªãŠã€é–¢ç¯€ã¨ãªã‚‹ãƒªãƒ³ã‚¯ã®ã¿ã‚’対象ã¨ã—ãŸã‚¤ãƒ³ãƒ‡ãƒƒã‚¯ã‚¹ã¨ã—ã¦ã€jointId ãŒã‚る。 \endif */ #include "Link.h" #include using namespace std; using namespace cnoid; Link::Link() { body = 0; index = -1; jointId = -1; jointType = FIXED_JOINT; parent = 0; sibling = 0; child = 0; isHighGainMode = false; defaultJointValue = 0.0; } Link::Link(const Link& org) : name_(org.name_) { body = 0; index = -1; // should be set by a Body object jointId = org.jointId; jointType = org.jointType; p = org.p; R = org.R; v = org.v; w = org.w; dv = org.dv; dw = org.dw; q = org.q; dq = org.dq; ddq = org.ddq; u = org.u; a = org.a; d = org.d; b = org.b; Rs = org.Rs; m = org.m; I = org.I; c = org.c; fext = org.fext; tauext = org.tauext; Jm2 = org.Jm2; ulimit = org.ulimit; llimit = org.llimit; uvlimit = org.uvlimit; lvlimit = org.lvlimit; defaultJointValue = org.defaultJointValue; torqueConst = org.torqueConst; encoderPulse = org.encoderPulse; Ir = org.Ir; gearRatio = org.gearRatio; gearEfficiency = org.gearEfficiency; rotorResistor = org.rotorResistor; isHighGainMode = org.isHighGainMode; if(org.coldetModel){ coldetModel = new ColdetModel(*org.coldetModel); } parent = child = sibling = 0; if(org.child){ stack children; for(Link* orgChild = org.child; orgChild; orgChild = orgChild->sibling){ children.push(orgChild); } while(!children.empty()){ addChild(new Link(*children.top())); children.pop(); } } } Link::~Link() { Link* link = child; while(link){ Link* linkToDelete = link; link = link->sibling; delete linkToDelete; } } void Link::addChild(Link* link) { if(link->parent){ link->parent->detachChild(link); } link->sibling = child; link->parent = this; child = link; link->setBodyIter(body); } /** A child link is detached from the link. The detached child link is *not* deleted by this function. If a link given by the parameter is not a child of the link, false is returned. */ bool Link::detachChild(Link* childToRemove) { bool removed = false; Link* link = child; Link* prevSibling = 0; while(link){ if(link == childToRemove){ removed = true; if(prevSibling){ prevSibling->sibling = link->sibling; } else { child = link->sibling; } break; } prevSibling = link; link = link->sibling; } if(removed){ childToRemove->parent = 0; childToRemove->sibling = 0; childToRemove->setBodyIter(0); } return removed; } void Link::setBodyIter(Body* body) { this->body = body; for(Link* link = child; link; link = link->sibling){ link->setBodyIter(body); } } std::ostream& operator<<(std::ostream &out, Link& link) { link.putInformation(out); return out; } void Link::putInformation(std::ostream& out) { out << "Link " << name() << " Link Index = " << index << ", Joint ID = " << jointId << "\n"; out << "Joint Type: "; switch(jointType) { case FREE_JOINT: out << "Free Joint\n"; break; case FIXED_JOINT: out << "Fixed Joint\n"; break; case ROTATIONAL_JOINT: out << "Rotational Joint\n"; out << "Axis = " << a << "\n"; break; case SLIDE_JOINT: out << "Slide Joint\n"; out << "Axis = " << d << "\n"; break; } out << "parent = " << (parent ? parent->name() : "null") << "\n"; out << "child = "; if(child){ Link* link = child; while(true){ out << link->name(); link = link->sibling; if(!link){ break; } out << ", "; } } else { out << "null"; } out << "\n"; out << "b = " << b << "\n"; out << "Rs = " << Rs << "\n"; out << "c = " << c << "\n"; out << "m = " << m << "\n"; out << "Ir = " << Ir << "\n"; out << "I = " << I << "\n"; out << "torqueConst = " << torqueConst << "\n"; out << "encoderPulse = " << encoderPulse << "\n"; out << "gearRatio = " << gearRatio << "\n"; out << "gearEfficiency = " << gearEfficiency << "\n"; out << "Jm2 = " << Jm2 << "\n"; out << "ulimit = " << ulimit << "\n"; out << "llimit = " << llimit << "\n"; out << "uvlimit = " << uvlimit << "\n"; out << "lvlimit = " << lvlimit << "\n"; if(false){ out << "R = " << R << "\n"; out << "p = " << p << ", wc = " << wc << "\n"; out << "v = " << v << ", vo = " << vo << ", dvo = " << dvo << "\n"; out << "w = " << w << ", dw = " << dw << "\n"; out << "u = " << u << ", q = " << q << ", dq = " << dq << ", ddq = " << ddq << "\n"; out << "fext = " << fext << ", tauext = " << tauext << "\n"; out << "sw = " << sw << ", sv = " << sv << "\n"; out << "Ivv = " << Ivv << "\n"; out << "Iwv = " << Iwv << "\n"; out << "Iww = " << Iww << "\n"; out << "cv = " << cv << ", cw = " << cw << "\n"; out << "pf = " << pf << ", ptau = " << ptau << "\n"; out << "hhv = " << hhv << ", hhw = " << hhw << "\n"; out << "uu = " << uu << ", dd = " << dd << "\n"; out << std::endl; } } void Link::calcSubMassCM() { subm = m; submwc = m*wc; if (child){ child->calcSubMassCM(); subm += child->subm; submwc += child->submwc; Link *l = child->sibling; while (l){ l->calcSubMassCM(); subm += l->subm; submwc += l->submwc; l = l->sibling; } } /* std::cout << "calcSubMassCM() : " << name << ", subm = " << subm << ", subCM = " << vector3(submwc/subm) << std::endl; */ } choreonoid-1.1.0+dfsg/src/Body/Link.h000066400000000000000000000140151207742442300173210ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_LINK_H_INCLUDED #define CNOID_BODY_LINK_H_INCLUDED #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class Link; } CNOID_EXPORT std::ostream& operator<<(std::ostream &out, cnoid::Link& link); namespace cnoid { class Body; class CNOID_EXPORT Link { public: Link(); Link(const Link& link); ~Link(); inline const std::string& name() { return name_; } inline void setName(const std::string& name){ name_ = name; } inline bool isValid() { return (index >= 0); } void addChild(Link* link); bool detachChild(Link* link); inline bool isRoot() { return !parent; } inline void setAttitude(const Matrix3& R) { this->R = R * Rs.transpose(); } inline Matrix3 attitude() { return this->R * Rs; } inline Matrix3 calcRfromAttitude(const Matrix3& R) { return R * Rs.transpose(); } /** @brief compute sum of m x wc of subtree @note assuming wc is already computed by Body::calcCM() */ void calcSubMassCM(); /** @deprecated use setAttitude(). */ inline void setSegmentAttitude(const Matrix3& R) { this->R = R * Rs.transpose(); } /** @deprecated use attitude(). */ inline Matrix3 segmentAttitude() { return this->R * Rs; } void updateColdetModelPosition() { coldetModel->setPosition(R, p); } Body* body; int index; int jointId; ///< jointId value written in a model file enum JointType { FIXED_JOINT, ///< fixed joint(0 dof) FREE_JOINT, /// 6-DOF root link ROTATIONAL_JOINT, ///< rotational joint (1 dof) SLIDE_JOINT ///< translational joint (1 dof) }; JointType jointType; Link* parent; Link* sibling; Link* child; Vector3 p; ///< position /** Internal world attitude. In the model computation, it corresponds to the identity matrix when all the joint angles of a robot are zero so that multiplication of local attitdue matrices can be omitted to simplify the computation. If you want to use the original coordinate in the model file, use setAttitude() and attitude() to access. */ Matrix3 R; Vector3 v; ///< linear velocity Vector3 w; ///< angular velocity, omega Vector3 dv; ///< linear acceleration Vector3 dw; ///< derivative of omega double q; ///< joint value double dq; ///< joint velocity double ddq; ///< joint acceleration double u; ///< joint torque Vector3 a; ///< rotational joint axis (self local) Vector3 d; ///< translation joint axis (self local) Vector3 b; ///< relative position (parent local) Matrix3 Rs; ///< relative attitude of the link segment (self local) double m; ///< mass Matrix3 I; ///< inertia tensor (self local, around c) Vector3 c; ///< center of mass (self local) Vector3 wc; ///< R * c + p Vector3 vo; ///< translation elements of spacial velocity Vector3 dvo; ///< derivative of vo /** A unit vector of angular velocity (the world coordinate) generated by the joint The value is parent->R * a when the joint is the rotational type. */ Vector3 sw; /** A unit vector of spatial velocity (the world coordinate) generated by the joint. The value is parent->R * d when the joint is the translation type. */ Vector3 sv; Vector3 cv; ///< dsv * dq (cross velocity term) Vector3 cw; ///< dsw * dq (cross velocity term) Vector3 fext; ///< external force Vector3 tauext; ///< external torque (around the world origin) // needed ? //Vector3 f; ///< force from the parent link //Vector3 tau; ///< torque from the parent link Matrix3 Iww; ///< bottm right block of the articulated inertia Matrix3 Iwv; ///< bottom left block (transpose of top right block) of the articulated inertia Matrix3 Ivv; ///< top left block of the articulated inertia Vector3 pf; ///< bias force (linear element) Vector3 ptau; ///< bias force (torque element) Vector3 hhv; ///< top block of Ia*s Vector3 hhw; ///< bottom bock of Ia*s double uu; double dd; ///< Ia*s*s^T double Jm2; ///< Equivalent rotor inertia: n^2*Jm [kg.m^2] double ulimit; ///< the upper limit of joint values double llimit; ///< the lower limit of joint values double uvlimit; ///< the upper limit of joint velocities double lvlimit; ///< the lower limit of joint velocities double defaultJointValue; double torqueConst; double encoderPulse; double Ir; ///< rotor inertia [kg.m^2] double gearRatio; double gearEfficiency; double rotorResistor; bool isHighGainMode; ColdetModelPtr coldetModel; struct ConstraintForce { Vector3 point; Vector3 force; }; typedef std::vector ConstraintForceArray; ConstraintForceArray constraintForces; double subm; ///< mass of subtree Vector3 submwc; ///< sum of m x wc of subtree private: std::string name_; Link& operator=(const Link& link); // no implementation is given to disable the copy operator void setBodyIter(Body* body); friend std::ostream& ::operator<<(std::ostream &out, Link& link); void putInformation(std::ostream& out); // for the iostream output }; }; #endif choreonoid-1.1.0+dfsg/src/Body/LinkGroup.cpp000066400000000000000000000033151207742442300206720ustar00rootroot00000000000000/* @author Shin'ichiro Nakaoka */ #include "LinkGroup.h" #include "Body.h" #include "Link.h" #include using namespace std; using namespace boost; using namespace cnoid; using namespace cnoid; LinkGroup::LinkGroup() { } LinkGroup::~LinkGroup() { } /** @param linkGroupSeq YAML node defining a ling group set. If linkGroupSeq.isValid() is false, a whole body group that contains all the links is created. */ LinkGroupPtr LinkGroup::create(BodyPtr body, const YamlSequence& linkGroupSeq) { LinkGroupPtr group = new LinkGroup(); group->setName("Whole Body"); if(!linkGroupSeq.isValid() || !group->load(body, linkGroupSeq)){ group->setFlatLinkList(body); } return group; } bool LinkGroup::load(BodyPtr& body, const YamlSequence& linkGroupSeq) { for(int i=0; i < linkGroupSeq.size(); ++i){ const YamlNode& node = linkGroupSeq[i]; YamlNodeType type = node.type(); if(type == YAML_SCALAR){ Link* link = body->link(node.toString()); if(!link){ return false; } elements.push_back(link->index); } else if(type == YAML_MAPPING){ const YamlMapping& group = *node.toMapping(); LinkGroupPtr linkGroup = new LinkGroup(); linkGroup->setName(group["name"]); if(linkGroup->load(body, *group["links"].toSequence())){ elements.push_back(linkGroup); } else { return false; } } else { return false; } } return true; } void LinkGroup::setFlatLinkList(BodyPtr& body) { for(int i=0; i < body->numLinks(); ++i){ elements.push_back(i); } } choreonoid-1.1.0+dfsg/src/Body/LinkGroup.h000066400000000000000000000030401207742442300203320ustar00rootroot00000000000000/* @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_LINK_GROUP_H_INCLUDED #define CNOID_BODY_LINK_GROUP_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { class YamlSequence; class Body; typedef boost::intrusive_ptr BodyPtr; class LinkGroup; typedef boost::intrusive_ptr LinkGroupPtr; class CNOID_EXPORT LinkGroup : public Referenced { typedef boost::variant Element; public: static LinkGroupPtr create(BodyPtr body, const YamlSequence& linkGroupSeq); LinkGroup(); virtual ~LinkGroup(); inline void setName(const std::string& name) { name_ = name; } inline const std::string& name() { return name_; } inline int numElements() { return elements.size(); } inline bool isSubGroup(int index) { return elements[index].which() == 0; } inline bool isLinkIndex(int index) { return elements[index].which() == 1; } inline LinkGroupPtr subGroup(int index) { return boost::get(elements[index]); } inline int linkIndex(int index) { return boost::get(elements[index]); } std::vector collectLinkIndices() const; std::vector collectGroups() const; private: std::string name_; std::vector elements; bool load(BodyPtr& body, const YamlSequence& linkGroupseq); void setFlatLinkList(BodyPtr& body); }; } #endif choreonoid-1.1.0+dfsg/src/Body/LinkPath.cpp000066400000000000000000000033771207742442300205020ustar00rootroot00000000000000/** \file \brief Implementations of the LinkPath class \author Shin'ichiro Nakaoka */ #include "LinkPath.h" #include "Link.h" #include using namespace std; using namespace cnoid; LinkPath::LinkPath() { } LinkPath::LinkPath(Link* base, Link* end) { find(base, end); } /// path from the root link LinkPath::LinkPath(Link* base) { find(base); } /// This method is disabled. void LinkPath::find(Link* root, bool doUpward, bool doDownward) { throw "The find method for LinkTraverse cannot be used in LinkPath"; } bool LinkPath::find(Link* base, Link* end) { links.clear(); numUpwardConnections = 0; bool found = findPathSub(base, 0, end, false); if(!found){ links.clear(); } return found; } bool LinkPath::findPathSub(Link* link, Link* prev, Link* end, bool isUpward) { links.push_back(link); if(isUpward){ ++numUpwardConnections; } if(link == end){ return true; } for(Link* child = link->child; child; child = child->sibling){ if(child != prev){ if(findPathSub(child, link, end, false)){ return true; } } } Link* parent = link->parent; if(parent && parent != prev){ if(findPathSub(parent, link, end, true)){ return true; } } links.pop_back(); if(isUpward){ --numUpwardConnections; } return false; } /// path from the root link void LinkPath::find(Link* end) { links.clear(); numUpwardConnections = 0; findPathFromRootSub(end); std::reverse(links.begin(), links.end()); } void LinkPath::findPathFromRootSub(Link* link) { links.push_back(link); if(link->parent){ findPathFromRootSub(link->parent); } } choreonoid-1.1.0+dfsg/src/Body/LinkPath.h000066400000000000000000000015351207742442300201410ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_LINK_PATH_H_INCLUDED #define CNOID_BODY_LINK_PATH_H_INCLUDED #include "LinkTraverse.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT LinkPath : public LinkTraverse { public: LinkPath(); LinkPath(Link* base, Link* end); LinkPath(Link* end); bool find(Link* base, Link* end); void find(Link* end); inline Link* baseLink() const { return links.front(); } inline Link* endLink() const { return links.back(); } private: virtual void find(Link* root, bool doUpward, bool doDownward); bool findPathSub(Link* link, Link* prev, Link* end, bool isForwardDirection); void findPathFromRootSub(Link* link); }; }; #endif choreonoid-1.1.0+dfsg/src/Body/LinkTraverse.cpp000066400000000000000000000130251207742442300213700ustar00rootroot00000000000000/** \file \brief Implementations of the LinkTraverse class \author Shin'ichiro Nakaoka */ #include "LinkTraverse.h" #include "Link.h" using namespace std; using namespace cnoid; LinkTraverse::LinkTraverse() { } LinkTraverse::LinkTraverse(int size) : links(size) { links.clear(); } LinkTraverse::LinkTraverse(Link* root, bool doUpward, bool doDownward) { find(root, doUpward, doDownward); } LinkTraverse::~LinkTraverse() { } void LinkTraverse::find(Link* root, bool doUpward, bool doDownward) { numUpwardConnections = 0; links.clear(); traverse(root, doUpward, doDownward, false, 0); } void LinkTraverse::traverse(Link* link, bool doUpward, bool doDownward, bool isUpward, Link* prev) { links.push_back(link); if(isUpward){ ++numUpwardConnections; } if(doUpward && link->parent){ traverse(link->parent, doUpward, true, true, link); } if(doDownward){ for(Link* child = link->child; child; child = child->sibling){ if(child != prev){ traverse(child, false, true, false, 0); } } } } void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) const { Vector3 arm; int i; for(i=1; i <= numUpwardConnections; ++i){ Link* link = links[i]; Link* child = links[i-1]; switch(child->jointType){ case Link::ROTATIONAL_JOINT: link->R.noalias() = child->R * AngleAxisd(child->q, child->a).inverse(); arm.noalias() = link->R * child->b; link->p = child->p - arm; if(calcVelocity){ child->sw.noalias() = link->R * child->a; link->w = child->w - child->dq * child->sw; link->v = child->v - link->w.cross(arm); if(calcAcceleration){ link->dw = child->dw - child->dq * child->w.cross(child->sw) - (child->ddq * child->sw); link->dv = child->dv - child->w.cross(child->w.cross(arm)) - child->dw.cross(arm); } } break; case Link::SLIDE_JOINT: link->R = child->R; arm.noalias() = link->R * (child->b + child->q * child->d); link->p = child->p - arm; if(calcVelocity){ child->sv.noalias() = link->R * child->d; link->w = child->w; link->v = child->v - child->dq * child->sv; if(calcAcceleration){ link->dw = child->dw; link->dv = child->dv - child->w.cross(child->w.cross(arm)) - child->dw.cross(arm) - 2.0 * child->dq * child->w.cross(child->sv) - child->ddq * child->sv; } } break; case Link::FIXED_JOINT: default: link->R = child->R; link->p.noalias() = child->p - (link->R * child->b); if(calcVelocity){ link->w = child->w; link->v = child->v; if(calcAcceleration){ link->dw = child->dw; link->dv = child->dv; } } break; } } int n = links.size(); for( ; i < n; ++i){ Link* link = links[i]; Link* parent = link->parent; switch(link->jointType){ case Link::ROTATIONAL_JOINT: link->R.noalias() = parent->R * AngleAxisd(link->q, link->a); arm.noalias() = parent->R * link->b; link->p = parent->p + arm; if(calcVelocity){ link->sw.noalias() = parent->R * link->a; link->w = parent->w + link->sw * link->dq; link->v = parent->v + parent->w.cross(arm); if(calcAcceleration){ link->dw = parent->dw + link->dq * parent->w.cross(link->sw) + (link->ddq * link->sw); link->dv = parent->dv + parent->w.cross(parent->w.cross(arm)) + parent->dw.cross(arm); } } break; case Link::SLIDE_JOINT: link->R = parent->R; arm.noalias() = parent->R * (link->b + link->q * link->d); link->p = parent->p + arm; if(calcVelocity){ link->sv.noalias() = parent->R * link->d; link->w = parent->w; link->v = parent->v + link->sv * link->dq; if(calcAcceleration){ link->dw = parent->dw; link->dv = parent->dv + parent->w.cross(parent->w.cross(arm)) + parent->dw.cross(arm) + 2.0 * link->dq * parent->w.cross(link->sv) + link->ddq * link->sv; } } break; case Link::FIXED_JOINT: default: link->R = parent->R; link->p.noalias() = parent->R * link->b + parent->p; if(calcVelocity){ link->w = parent->w; link->v = parent->v; if(calcAcceleration){ link->dw = parent->dw; link->dv = parent->dv; } } break; } } } std::ostream& operator<<(std::ostream& os, LinkTraverse& traverse) { int n = traverse.numLinks(); for(int i=0; i < n; ++i){ Link* link = traverse[i]; os << link->name(); if(i != n){ os << (traverse.isDownward(i) ? " => " : " <= "); } } os << std::endl; return os; } choreonoid-1.1.0+dfsg/src/Body/LinkTraverse.h000066400000000000000000000041401207742442300210330ustar00rootroot00000000000000/** \file \brief The header file of the LinkTraverse class \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_LINK_TRAVERSE_H_INCLUDED #define CNOID_BODY_LINK_TRAVERSE_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class Link; class CNOID_EXPORT LinkTraverse { public: LinkTraverse(); LinkTraverse(int size); LinkTraverse(Link* root, bool doUpward = false, bool doDownward = true); virtual ~LinkTraverse(); virtual void find(Link* root, bool doUpward = false, bool doDownward = true); inline int numLinks() const { return links.size(); } inline bool empty() const { return links.empty(); } inline size_t size() const { return links.size(); } inline Link* rootLink() const { return (links.empty() ? 0 : links.front()); } inline Link* link(int index) const { return links[index]; } inline Link* operator[] (int index) const { return links[index]; } inline std::vector::const_iterator begin() const { return links.begin(); } inline std::vector::const_iterator end() const { return links.end(); } /** If the connection from the queried link to the next link is downward (forward) direction, the method returns true. Otherwise, returns false. The range of valid indices is 0 to (numLinks() - 2). */ inline bool isDownward(int index) const { return (index >= numUpwardConnections); } void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false) const; protected: std::vector links; int numUpwardConnections; private: void traverse(Link* link, bool doUpward, bool doDownward, bool isUpward, Link* prev); }; }; CNOID_EXPORT std::ostream& operator<<(std::ostream& os, cnoid::LinkTraverse& traverse); #endif choreonoid-1.1.0+dfsg/src/Body/MassMatrix.cpp000066400000000000000000000065401207742442300210530ustar00rootroot00000000000000/** \author Shin'ichiro Nakaoka */ #include "MassMatrix.h" #include "Link.h" #include "InverseDynamics.h" using namespace cnoid; namespace { void setColumnOfMassMatrix(const BodyPtr& body, MatrixXd& out_M, int column) { Vector3 f, tau; calcInverseDynamics(body, body->rootLink(), f, tau); if(!body->isStaticModel()){ tau -= body->rootLink()->p.cross(f); out_M.block<6, 1>(0, column) << f, tau; } const int n = body->numJoints(); for(int i = 0; i < n; ++i){ Link* joint = body->joint(i); out_M(i + 6, column) = joint->u; } } } namespace cnoid { /** calculate the mass matrix using the unit vector method \todo replace the unit vector method here with a more efficient method The motion equation (dv != dvo) | | | dv | | | | fext | | out_M | * | dw | + | b1 | = | tauext | | | |ddq | | | | u | */ void calcMassMatrix(const BodyPtr& body, Eigen::MatrixXd& out_M) { const Vector3 g(0, 0, 9.8); int nJ = body->numJoints(); int totaldof = nJ; if(!body->isStaticModel()){ totaldof += 6; } Link* rootLink = body->rootLink(); out_M.resize(totaldof, totaldof); // preserve and clear the joint accelerations VectorXd ddqorg(nJ); VectorXd uorg(nJ); for(int i = 0; i < nJ; ++i){ Link* joint = body->joint(i); ddqorg[i] = joint->ddq; uorg [i] = joint->u; joint->ddq = 0.0; } // preserve and clear the root link acceleration const Vector3 dvoorg = rootLink->dvo; const Vector3 dworg = rootLink->dw; const Vector3 root_w_x_v = rootLink->w.cross(rootLink->vo + rootLink->w.cross(rootLink->p)); rootLink->dvo = g - root_w_x_v; // dv = g, dw = 0 rootLink->dw.setZero(); MatrixXd b1(totaldof, 1); setColumnOfMassMatrix(body, b1, 0); if(!body->isStaticModel()){ for(int i=0; i < 3; ++i){ rootLink->dvo[i] += 1.0; setColumnOfMassMatrix(body, out_M, i); rootLink->dvo[i] -= 1.0; } for(int i=0; i < 3; ++i){ rootLink->dw[i] = 1.0; const Vector3 dw_x_p = rootLink->dw.cross(rootLink->p); // spatial acceleration caused by ang. acc. rootLink->dvo -= dw_x_p; setColumnOfMassMatrix(body, out_M, i + 3); rootLink->dvo += dw_x_p; rootLink->dw[i] = 0.0; } } for(int i = 0; i < nJ; ++i){ Link* joint = body->joint(i); joint->ddq = 1.0; int j = i + 6; setColumnOfMassMatrix(body, out_M, j); out_M(j, j) += joint->Jm2; // motor inertia joint->ddq = 0.0; } // subtract the constant term for(int i = 0; i < out_M.cols(); ++i){ out_M.col(i) -= b1; } // recover state for(int i = 0; i < nJ; ++i){ Link* joint = body->joint(i); joint->ddq = ddqorg[i]; joint->u = uorg [i]; } rootLink->dvo = dvoorg; rootLink->dw = dworg; } } choreonoid-1.1.0+dfsg/src/Body/MassMatrix.h000066400000000000000000000004211207742442300205100ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_MASS_MATRIX_H_INCLUDED #define CNOID_BODY_MASS_MATRIX_H_INCLUDED #include "Body.h" #include "exportdecl.h" namespace cnoid { CNOID_EXPORT void calcMassMatrix(const BodyPtr& body, MatrixXd& out_M); } #endif choreonoid-1.1.0+dfsg/src/Body/ModelNodeSet.cpp000066400000000000000000000335101207742442300213020ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "ModelNodeSet.h" #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { typedef void (ModelNodeSetImpl::*ProtoCheckFunc)(void); struct ProtoInfo { ProtoInfo() { } ProtoInfo(int id, ProtoCheckFunc func) : id(id), protoCheckFunc(func) { } int id; ProtoCheckFunc protoCheckFunc; }; typedef map ProtoNameToInfoMap; ProtoNameToInfoMap protoNameToInfoMap; } namespace cnoid { class ModelNodeSetImpl { public: ModelNodeSetImpl(ModelNodeSet* self); bool loadModelFile(const std::string& filename); ModelNodeSet* self; int numJointNodes; VrmlProtoInstancePtr humanoidNode; JointNodeSetPtr rootJointNodeSet; int messageIndent; VrmlProtoPtr protoToCheck; enum { PROTO_UNDEFINED = 0, PROTO_HUMANOID, PROTO_JOINT, PROTO_SEGMENT, PROTO_SENSOR, PROTO_HARDWARECOMPONENT, NUM_PROTOS }; typedef std::bitset ProtoIdSet; void extractHumanoidNode(VrmlParser& parser); void throwExceptionOfIllegalField(const std::string& name, const char* label); template VrmlVariantField* addField(const std::string& name, const TValue& defaultValue) { VrmlVariantField* field = protoToCheck->findField(name); if(!field){ field = &protoToCheck->field(name); (*field) = defaultValue; } else if(field->type() != typeid(TValue)){ throwExceptionOfIllegalField(name, labelOfVrmlFieldType()); } return field; } template VrmlVariantField* addField(const std::string& name) { return addField(name, TValue()); } template void requireField(const std::string& name){ VrmlVariantField* field = protoToCheck->findField(name); if(!field || field->type() != typeid(TValue)){ throwExceptionOfIllegalField(name, labelOfVrmlFieldType()); } } void checkHumanoidProto(); void checkJointProto(); void checkSegmentProto(); void checkSensorProtoCommon(); void checkHardwareComponentProto(); void extractJointNodes(); JointNodeSetPtr addJointNodeSet(VrmlProtoInstancePtr jointNode); void extractChildNodes (JointNodeSetPtr jointNodeSet, MFNode& childNodes, const ProtoIdSet acceptableProtoIds, const Affine3& T); void putMessage(const std::string& message); }; } ModelNodeSet::ModelNodeSet() { impl = new ModelNodeSetImpl(this); } ModelNodeSetImpl::ModelNodeSetImpl(ModelNodeSet* self) : self(self) { numJointNodes = 0; messageIndent = 0; if(protoNameToInfoMap.empty()){ protoNameToInfoMap["Humanoid"] = ProtoInfo(PROTO_HUMANOID, &ModelNodeSetImpl::checkHumanoidProto); protoNameToInfoMap["Joint"] = ProtoInfo(PROTO_JOINT, &ModelNodeSetImpl::checkJointProto); protoNameToInfoMap["Segment"] = ProtoInfo(PROTO_SEGMENT, &ModelNodeSetImpl::checkSegmentProto); protoNameToInfoMap["ForceSensor"] = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon); protoNameToInfoMap["Gyro"] = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon); protoNameToInfoMap["AccelerationSensor"] = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon); protoNameToInfoMap["VisionSensor"] = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon); protoNameToInfoMap["RangeSensor"] = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon); protoNameToInfoMap["HardwareComponent"] = ProtoInfo(PROTO_HARDWARECOMPONENT, &ModelNodeSetImpl::checkHardwareComponentProto); } } ModelNodeSet::~ModelNodeSet() { delete impl; } int ModelNodeSet::numJointNodes() { return impl->numJointNodes; } VrmlProtoInstancePtr ModelNodeSet::humanoidNode() { return impl->humanoidNode; } JointNodeSetPtr ModelNodeSet::rootJointNodeSet() { return impl->rootJointNodeSet; } bool ModelNodeSet::loadModelFile(const std::string& filename) { return impl->loadModelFile(filename); } bool ModelNodeSetImpl::loadModelFile(const std::string& filename) { numJointNodes = 0; humanoidNode = 0; rootJointNodeSet.reset(); messageIndent = 0; try { VrmlParser parser; parser.load(filename); extractHumanoidNode(parser); } catch(EasyScanner::Exception& ex){ throw ModelNodeSet::Exception(ex.getFullMessage()); } return (humanoidNode && rootJointNodeSet); } void ModelNodeSetImpl::extractHumanoidNode(VrmlParser& parser) { while(VrmlNodePtr node = parser.readNode()){ if(node->isCategoryOf(PROTO_DEF_NODE)){ protoToCheck = static_pointer_cast(node); ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoToCheck->protoName); if(p != protoNameToInfoMap.end()){ ProtoInfo& info = p->second; (this->*info.protoCheckFunc)(); } } else if(node->isCategoryOf(PROTO_INSTANCE_NODE)){ VrmlProtoInstancePtr instance = static_pointer_cast(node); if(instance->proto->protoName == "Humanoid") { humanoidNode = instance; } } } if(humanoidNode){ putMessage("Humanoid node"); extractJointNodes(); } else { throw ModelNodeSet::Exception("Humanoid node is not found"); } } void ModelNodeSetImpl::throwExceptionOfIllegalField(const std::string& name, const char* label) { format message("Proto \"%1%\" must have the \"%2%\" field of %3% type"); throw ModelNodeSet::Exception(str(message % protoToCheck->protoName % name % label)); } void ModelNodeSetImpl::checkHumanoidProto() { // necessary fields requireField("center"); requireField("humanoidBody"); requireField("rotation"); requireField("translation"); // optional fields addField("info"); addField("name"); addField("version"); addField("scaleOrientation"); addField("scale", SFVec3f::Constant(1.0)); } void ModelNodeSetImpl::checkJointProto() { // necessary fields requireField("center"); requireField("children"); requireField("rotation"); requireField("translation"); requireField("jointType"); requireField("jointId"); VrmlVariantField* field; field = protoToCheck->findField("jointAxis"); if(!field){ throw ModelNodeSet::Exception ("Prototype of Humanoid must have the \"jointAxis\" field"); } if(field->type() != typeid(SFString) && field->type() != typeid(SFVec3f)){ throw ModelNodeSet::Exception ("The type of \"jointAxis\" field in \"Humanoid\" prototype must be SFString or SFVec3f"); } // optional fields addField("llimit"); addField("ulimit"); addField("lvlimit"); addField("uvlimit"); addField("limitOrientation"); addField("name"); addField("gearRatio", 1.0); addField("rotorInertia", 0.0); addField("rotorResistor", 0.0); addField("torqueConst", 1.0); addField("encoderPulse", 1.0); addField("jointValue", 0.0); addField("scale", SFVec3f::Constant(1.0)); if(protoToCheck->findField("equivalentInertia")){ putMessage("The \"equivalentInertia\" field of the Joint node is obsolete."); } } void ModelNodeSetImpl::checkSegmentProto() { requireField("centerOfMass"); requireField("mass"); requireField("momentsOfInertia"); addField("name"); } void ModelNodeSetImpl::checkSensorProtoCommon() { requireField("sensorId"); requireField("translation"); requireField("rotation"); } void ModelNodeSetImpl::checkHardwareComponentProto() { requireField("id"); requireField("translation"); requireField("rotation"); requireField("url"); } void ModelNodeSetImpl::extractJointNodes() { MFNode& nodes = get(humanoidNode->fields["humanoidBody"]); if(nodes.size() > 1){ throw ModelNodeSet::Exception ("The Humanoid node must have a unique Joint node in its \"humanoidBody\" field."); } else if(nodes.size() == 1){ if(nodes[0]->isCategoryOf(PROTO_INSTANCE_NODE)){ VrmlProtoInstancePtr jointNode = dynamic_pointer_cast(nodes[0]); if(jointNode && jointNode->proto->protoName == "Joint"){ rootJointNodeSet = addJointNodeSet(jointNode); } } } if(!rootJointNodeSet){ throw ModelNodeSet::Exception ("The Humanoid node does not have a Joint node in its \"humanoidBody\" field."); } } JointNodeSetPtr ModelNodeSetImpl::addJointNodeSet(VrmlProtoInstancePtr jointNode) { numJointNodes++; putMessage(string("Joint node") + jointNode->defName); JointNodeSetPtr jointNodeSet(new JointNodeSet()); jointNodeSet->jointNode = jointNode; MFNode& childNodes = get(jointNode->fields["children"]); ProtoIdSet acceptableProtoIds; acceptableProtoIds.set(PROTO_JOINT); acceptableProtoIds.set(PROTO_SEGMENT); acceptableProtoIds.set(PROTO_SENSOR); acceptableProtoIds.set(PROTO_HARDWARECOMPONENT); Affine3 T(Affine3::Identity()); extractChildNodes(jointNodeSet, childNodes, acceptableProtoIds, T); return jointNodeSet; } void ModelNodeSetImpl::extractChildNodes (JointNodeSetPtr jointNodeSet, MFNode& childNodes, const ProtoIdSet acceptableProtoIds, const Affine3& T) { for(size_t i = 0; i < childNodes.size(); i++){ VrmlNode* childNode = childNodes[i].get(); const Affine3* pT; if(childNode->isCategoryOf(GROUPING_NODE)){ VrmlGroup* groupNode = static_cast(childNode); VrmlTransform* transformNode = dynamic_cast(groupNode); Affine3 T2; if(transformNode){ T2 = T * transformNode->toAffine3d(); pT = &T2; } else { pT = &T; } extractChildNodes(jointNodeSet, groupNode->getChildren(), acceptableProtoIds, *pT); } else if(childNode->isCategoryOf(PROTO_INSTANCE_NODE)){ VrmlProtoInstance* protoInstance = static_cast(childNode); int id = PROTO_UNDEFINED; bool doTraverseChildren = false; ProtoIdSet acceptableChildProtoIds(acceptableProtoIds); const string& protoName = protoInstance->proto->protoName; ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoName); if(p == protoNameToInfoMap.end()){ doTraverseChildren = true; } else { id = p->second.id; if(!acceptableProtoIds.test(id)){ throw ModelNodeSet::Exception(protoName + " node is not in a correct place."); } } messageIndent += 2; switch(id){ case PROTO_JOINT: if(T.matrix() != Affine3::MatrixType::Identity()){ throw ModelNodeSet::Exception(protoName + " node is not in a correct place."); } jointNodeSet->childJointNodeSets.push_back(addJointNodeSet(protoInstance)); break; case PROTO_SENSOR: if(T.matrix() != Affine3::MatrixType::Identity()){ throw ModelNodeSet::Exception(protoName + " node is not in a correct place."); } jointNodeSet->sensorNodes.push_back(protoInstance); putMessage(protoName + protoInstance->defName); break; case PROTO_HARDWARECOMPONENT: if(T.matrix() != Affine3::MatrixType::Identity()){ throw ModelNodeSet::Exception(protoName + " node is not in a correct place."); } jointNodeSet->hwcNodes.push_back(protoInstance); putMessage(protoName + protoInstance->defName); break; case PROTO_SEGMENT: { jointNodeSet->segmentNodes.push_back(protoInstance); jointNodeSet->transforms.push_back(T); putMessage(string("Segment node ") + protoInstance->defName); doTraverseChildren = true; acceptableChildProtoIds.reset(); acceptableChildProtoIds.set(PROTO_SENSOR); } break; default: break; } if(doTraverseChildren){ MFNode& childNodes = get(protoInstance->fields["children"]); extractChildNodes(jointNodeSet, childNodes, acceptableChildProtoIds, T); } messageIndent -= 2; } } } void ModelNodeSetImpl::putMessage(const std::string& message) { if(!self->sigMessage.empty()) { string space(messageIndent, ' '); self->sigMessage(space + message + "\n"); } } choreonoid-1.1.0+dfsg/src/Body/ModelNodeSet.h000066400000000000000000000037131207742442300207510ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_MODEL_NODE_SET_H_INCLUDED #define CNOID_BODY_MODEL_NODE_SET_H_INCLUDED #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class VrmlParser; class JointNodeSet; typedef boost::shared_ptr JointNodeSetPtr; class JointNodeSet { public: typedef std::vector > Affine3Array; VrmlProtoInstancePtr jointNode; std::vector childJointNodeSets; Affine3Array transforms; std::vector segmentNodes; std::vector sensorNodes; std::vector hwcNodes; }; typedef std::vector JointNodeSetArray; class ModelNodeSetImpl; class CNOID_EXPORT ModelNodeSet { public: ModelNodeSet(); virtual ~ModelNodeSet(); bool loadModelFile(const std::string& filename); int numJointNodes(); VrmlProtoInstancePtr humanoidNode(); JointNodeSetPtr rootJointNodeSet(); /** @if jp 読ã¿è¾¼ã¿é€²è¡ŒçŠ¶æ³ã®ãƒ¡ãƒƒã‚»ãƒ¼ã‚¸ã‚’出力ã™ã‚‹ãŸã‚ã®ã‚·ã‚°ãƒŠãƒ«. @note エラー発生時ã®ãƒ¡ãƒƒã‚»ãƒ¼ã‚¸ã¯ã“ã®ã‚·ã‚°ãƒŠãƒ«ã§ã¯ãªã例外ã«ã‚ˆã£ã¦å‡¦ç†ã•ã‚Œã‚‹ã€‚ @endif */ boost::signal sigMessage; class Exception { public: Exception(const std::string& description) : description(description) { } const char* what() const { return description.c_str(); } private: std::string description; }; private: ModelNodeSetImpl* impl; }; typedef boost::shared_ptr ModelNodeSetPtr; }; #endif choreonoid-1.1.0+dfsg/src/Body/OnlineViewerUtil.cpp000066400000000000000000000034301207742442300222220ustar00rootroot00000000000000 #include "OnlineViewerUtil.h" //using namespace cnoid; using namespace OpenHRP; OnlineViewer_var getOnlineViewer(CosNaming::NamingContext_var cxt) { CosNaming::Name ncName; ncName.length(1); ncName[0].id = CORBA::string_dup("OnlineViewer"); ncName[0].kind = CORBA::string_dup(""); OnlineViewer_var onlineViewer = NULL; try { onlineViewer = OnlineViewer::_narrow(cxt->resolve(ncName)); } catch(const CosNaming::NamingContext::NotFound &exc) { std::cerr << "OnlineViewer not found: "; switch(exc.why) { case CosNaming::NamingContext::missing_node: std::cerr << "Missing Node" << std::endl; case CosNaming::NamingContext::not_context: std::cerr << "Not Context" << std::endl; break; case CosNaming::NamingContext::not_object: std::cerr << "Not Object" << std::endl; break; } return 0; } catch(CosNaming::NamingContext::CannotProceed &exc) { std::cerr << "Resolve OnlineViewer CannotProceed" << std::endl; return 0; } catch(CosNaming::NamingContext::AlreadyBound &exc) { std::cerr << "Resolve OnlineViewer InvalidName" << std::endl; return 0; } return onlineViewer; } OnlineViewer_var getOnlineViewer(CORBA_ORB_var orb) { CosNaming::NamingContext_var cxt; try { CORBA::Object_var nS = orb->resolve_initial_references("NameService"); cxt = CosNaming::NamingContext::_narrow(nS); } catch(CORBA::SystemException& ex) { std::cerr << "NameService doesn't exist" << std::endl; return 0; } return getOnlineViewer(cxt); } OnlineViewer_var getOnlineViewer(int argc, char **argv) { CORBA_ORB_var orb = CORBA::ORB_init(argc, argv); return getOnlineViewer(orb); } choreonoid-1.1.0+dfsg/src/Body/OnlineViewerUtil.h000066400000000000000000000007341207742442300216730ustar00rootroot00000000000000 #ifndef ONLINEVIEWERCLIENT_H #define ONLINEVIEWERCLIENT_H #pragma warning(disable:4996) #include "config.h" #include #include #include #include #include #include namespace cnoid { HRP_UTIL_EXPORT OpenHRP::OnlineViewer_var getOnlineViewer(int argc, char **argv); HRP_UTIL_EXPORT OpenHRP::OnlineViewer_var getOnlineViewer(CORBA_ORB_var orb); }; #endif choreonoid-1.1.0+dfsg/src/Body/PenetrationBlocker.cpp000066400000000000000000000102501207742442300225460ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "PenetrationBlocker.h" #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } namespace cnoid { class PenetrationBlockerImpl { public: PenetrationBlockerImpl(Link* targetLink); Link* targetLink; ColdetModelPtr targetModel; std::vector coldetModelPairs; double targetDepth; Vector3 pPrevGiven; bool isPrevBlocked; Vector3 prevBlockedPosition; Vector3 prevBlockedNormal; struct LinkInfo { Link* link; // A coldet model is copied from the original one ColdetModelPtr coldetModel; }; std::vector opponentLinkInfos; void addOpponentLink(Link* link); void start(); bool adjust(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection); bool adjustSub(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection); }; } PenetrationBlocker::PenetrationBlocker(Link* targetLink) { impl = new PenetrationBlockerImpl(targetLink); } PenetrationBlockerImpl::PenetrationBlockerImpl(Link* targetLink) : targetLink(targetLink) { if(targetLink->coldetModel){ targetModel = new ColdetModel(*targetLink->coldetModel); } pPrevGiven = targetLink->p; targetDepth = 0.001; isPrevBlocked = false; } void PenetrationBlocker::addOpponentLink(Link* link) { impl->addOpponentLink(link); } void PenetrationBlockerImpl::addOpponentLink(Link* link) { if(link->coldetModel){ ColdetModelPtr opponendModel = new ColdetModel(*link->coldetModel); coldetModelPairs.push_back(new ColdetModelPair(opponendModel, targetModel)); LinkInfo info; info.link = link; info.coldetModel = opponendModel; opponentLinkInfos.push_back(info); } } void PenetrationBlocker::setDepth(double depth) { impl->targetDepth = depth; } void PenetrationBlocker::start() { impl->isPrevBlocked = false; } bool PenetrationBlocker::adjust(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection) { return impl->adjust(io_p, R, pushDirection); } bool PenetrationBlockerImpl::adjust(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection) { if(isPrevBlocked){ if((io_p - prevBlockedPosition).dot(prevBlockedNormal) < 0.0){ io_p = prevBlockedPosition; return true; } } return adjustSub(io_p, R, pushDirection); } bool PenetrationBlockerImpl::adjustSub(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection) { bool blocked = false; const Vector3 s = pushDirection.normalized(); for(size_t i=0; i < opponentLinkInfos.size(); ++i){ LinkInfo& info = opponentLinkInfos[i]; info.coldetModel->setPosition(info.link->R, info.link->p); } int loop; Vector3 maxnormal(Vector3::Zero()); for(loop = 0; loop < 100; ++loop){ targetModel->setPosition(R, io_p); double maxsdepth = 0.0; double maxdepth = 0.0; for(size_t i=0; i < coldetModelPairs.size(); ++i){ ColdetModelPair& modelPair = *coldetModelPairs[i]; const std::vector& cols = modelPair.detectCollisions(); for(size_t j=0; j < cols.size(); ++j){ const collision_data& col = cols[j]; if(col.depth > targetDepth){ double d = -col.n_vector.dot(s); if(d > 0.0){ double sdepth = col.depth * d; if(sdepth > maxsdepth){ maxsdepth = sdepth; maxdepth = col.depth; maxnormal = col.n_vector; } } } } } if(maxsdepth > 0.0){ io_p += (maxdepth - targetDepth) * maxnormal; blocked = true; } else { break; } } isPrevBlocked = blocked; prevBlockedPosition = io_p; prevBlockedNormal = maxnormal; return blocked; } choreonoid-1.1.0+dfsg/src/Body/PenetrationBlocker.h000066400000000000000000000013021207742442300222110ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_PENETRATION_BLOCKER_H_INCLUDED #define CNOID_BODY_PENETRATION_BLOCKER_H_INCLUDED #include "Link.h" #include #include "exportdecl.h" namespace cnoid { class PenetrationBlockerImpl; class CNOID_EXPORT PenetrationBlocker { public: PenetrationBlocker(Link* targetLink); void addOpponentLink(Link* link); void setDepth(double depth); void start(); bool adjust(Vector3& io_p, const Matrix3& R, const Vector3& pushDirection); private: PenetrationBlockerImpl* impl; }; typedef boost::shared_ptr PenetrationBlockerPtr; } #endif choreonoid-1.1.0+dfsg/src/Body/PinDragIK.cpp000066400000000000000000000616341207742442300205400ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "PinDragIK.h" #include "Link.h" #include "JointPath.h" #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool SOLVE_CONSTRAINTS_BY_SR_INVERSE = false; const bool SOLVE_CONSTRAINTS_BY_SVD = !SOLVE_CONSTRAINTS_BY_SR_INVERSE; double calcLU(int n, MatrixXd& a, std::vector& pivots); void solveByLU(int n, MatrixXd& a, std::vector& pivots, const MatrixXd::ColXpr& x, const VectorXd& b); bool makeInverseMatrix(int n, MatrixXd& org, MatrixXd& inv, double minValidDet); bool makePseudoInverseType1(int m, int n, const MatrixXd& J, MatrixXd& Jinv, MatrixXd& JJ, MatrixXd& JJinv, double minValidDet); bool makePseudoInverseType2(int m, int n, const MatrixXd& J, MatrixXd& Jinv, MatrixXd& JJ, MatrixXd& JJinv, const VectorXd& weights, double minValidDet); bool makeSRInverseMatrix(int m, int n, const MatrixXd& J, MatrixXd& Jinv, MatrixXd& JJ, MatrixXd& JJ2, MatrixXd& JJinv, std::vector& pivots, double srk0, double srw0); } namespace cnoid { class PinDragIKImpl { public: PinDragIKImpl(BodyPtr body); BodyPtr body_; int maxIteration; //! calculation is iterated until the erro is less than this value double ikErrorSqrThresh; // number of joints (body->numJoints()) int NJ; // dimension of joint space (this can includes elements of 6-DOF free root) int N; // dimension of target space. 3 means position only, 6 means position and orientation. int M; Link* baseLink; Link* targetLink; bool isJointRangeConstraintsEnabled; VectorXd q_org; Vector3 base_p_org; Matrix3 base_R_org; MatrixXd J; // Jacobian Matrix (M x N) MatrixXd Jinv; // J^-1 (N x N) or pseudo inverse of J (N x M) // temporary matrix variables MatrixXd JJ; // (J^T * J) (N < M, N x N) or (J * J^T) (N > M, M x M) MatrixXd JJinv; // JJ^-1 VectorXd dr_p; // d(x, y, z, w_x, w_y, w_z) / dt double minValidDet; // dimension of constrained variables (vector p_aux) int C; // Matrices MatrixXd Jaux; // Jacobian Matrix (C x N) MatrixXd Jauxinv; MatrixXd W; MatrixXd S; // (C x N) MatrixXd Sinv; MatrixXd JJ2; // Joint space vector to solve (size N) // This vector includes elements of 6-DOF root joint (dx, dy, dz, OmegaX, OmegaY, OmegaZ) // in the tail when root free model is enabled. VectorXd dq; // weight of joint space vector (size N) // this includes 6-DOF root joint like dq VectorXd qWeights; VectorXd y; // size N std::vector pivots; bool isBaseLinkFreeMode; bool isTargetAttitudeEnabled; LinkTraverse fkTraverse; JointPath targetJointPath; struct PinProperty { double weight; InverseKinematics::AxisSet axes; JointPath jointPath; Vector3 p; Matrix3 R; Vector3 prevStep_p; Matrix3 prevStep_R; }; typedef std::map PinPropertyMap; PinPropertyMap pinPropertyMap; std::vector constraintWeightsSqrt; // size C struct JointConstrain { JointConstrain(int jointId, double dq) : jointId(jointId), dq(dq) { } int jointId; double dq; }; std::vector jointConstraints; VectorXd dPaux; // size C double srk0; // k of the singular point double srw0; // threshold value to calc k enum IKStepResult { ERROR, PINS_NOT_CONVERGED, PINS_CONVERGED }; void setBaseLink(Link* baseLink); void setFreeRootWeight(double translation, double rotation); void setTargetLink(Link* targetLink, bool isAttitudeEnabled); void setJointWeight(int jointId, double weight); void setPin(Link* link, InverseKinematics::AxisSet axes, double weight); InverseKinematics::AxisSet pinAxes(Link* link); void setIKErrorThresh(double e); void setSRInverseParameters(double k0, double w0); void enableJointRangeConstraints(bool on); bool initialize(); bool calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R); IKStepResult calcOneStep(const Vector3& v, const Vector3& omega); void solveConstraints(); void setJacobianForOnePath(MatrixXd& J, int row, JointPath& jointPath, int axes); void setJacobianForFreeRoot(MatrixXd& J, int row, JointPath& jointPath, int axes); void addPinConstraints(); void addJointRangeConstraints(); int solveLinearEquationWithSVD(MatrixXd& A, VectorXd& b, VectorXd& x, double sv_ratio); }; } PinDragIK::PinDragIK(BodyPtr body) { impl = new PinDragIKImpl(body); } PinDragIKImpl::PinDragIKImpl(BodyPtr body) : body_(body), NJ(body->numJoints()), q_org(NJ), qWeights(NJ + 6) { M = 0; N = 0; maxIteration = 50; //minValidDet = 1.0e-12; //minValidDet = 1.0e-5 minValidDet = 1.0e-9; setBaseLink(0); setTargetLink(0, false); qWeights.head(qWeights.size() - 6).fill(1.0); setFreeRootWeight(30.0, 1.0); setIKErrorThresh(1.0e-5); setSRInverseParameters(0.1, 0.001); //enableJointRangeConstraints(true); enableJointRangeConstraints(false); } PinDragIK::~PinDragIK() { delete impl; } BodyPtr PinDragIK::body() const { return impl->body_; } void PinDragIK::setBaseLink(Link* baseLink) { impl->setBaseLink(baseLink); } //! if root is zero, root is virtual free 6-DOF link on the top link void PinDragIKImpl::setBaseLink(Link* baseLink) { if(baseLink){ this->baseLink = baseLink; isBaseLinkFreeMode = false; } else { this->baseLink = body_->rootLink(); isBaseLinkFreeMode = true; } } void PinDragIK::setFreeRootWeight(double translation, double rotation) { impl->setFreeRootWeight(translation, rotation); } void PinDragIKImpl::setFreeRootWeight(double translation, double rotation) { for(int i=0; i < 3; i++){ qWeights(NJ + i) = translation; qWeights(NJ + 3 + i) = rotation; } } void PinDragIK::setTargetLink(Link* targetLink, bool isAttitudeEnabled) { impl->setTargetLink(targetLink, isAttitudeEnabled); } void PinDragIKImpl::setTargetLink(Link* targetLink, bool isAttitudeEnabled) { this->targetLink = targetLink; isTargetAttitudeEnabled = isAttitudeEnabled; M = isAttitudeEnabled ? 6 : 3; } void PinDragIK::setJointWeight(int jointId, double weight) { impl->setJointWeight(jointId, weight); } void PinDragIKImpl::setJointWeight(int jointId, double weight) { qWeights(jointId) = weight; } void PinDragIK::setPin(Link* link, InverseKinematics::AxisSet axes, double weight) { impl->setPin(link, axes, weight); } void PinDragIKImpl::setPin(Link* link, InverseKinematics::AxisSet axes, double weight) { if(link){ if(axes == InverseKinematics::NO_AXES){ pinPropertyMap.erase(link); } else { PinProperty& property = pinPropertyMap[link]; property.axes = axes; property.weight = weight; } } } InverseKinematics::AxisSet PinDragIK::pinAxes(Link* link) { return impl->pinAxes(link); } InverseKinematics::AxisSet PinDragIKImpl::pinAxes(Link* link) { PinPropertyMap::iterator p = pinPropertyMap.find(link); if(p == pinPropertyMap.end()){ return PinDragIK::NO_AXES; } return p->second.axes; } void PinDragIK::clearPins() { impl->pinPropertyMap.clear(); } int PinDragIK::numPinnedLinks() { return impl->pinPropertyMap.size(); } void PinDragIK::setIKErrorThresh(double e) { impl->setIKErrorThresh(e); } void PinDragIKImpl::setIKErrorThresh(double e) { ikErrorSqrThresh = e * e; } bool PinDragIK::hasAnalyticalIK() { return false; } InverseKinematics::AxisSet PinDragIK::targetAxes() const { return impl->isTargetAttitudeEnabled ? InverseKinematics::TRANSFORM_6D : InverseKinematics::TRANSLATION_3D; } void PinDragIK::setSRInverseParameters(double k0, double w0) { impl->setSRInverseParameters(k0, w0); } void PinDragIKImpl::setSRInverseParameters(double k0, double w0) { srk0 = k0; srw0 = w0; } void PinDragIK::enableJointRangeConstraints(bool on) { impl->enableJointRangeConstraints(on); } void PinDragIKImpl::enableJointRangeConstraints(bool on) { isJointRangeConstraintsEnabled = on; } bool PinDragIK::initialize() { return impl->initialize(); } bool PinDragIKImpl::initialize() { if(!targetLink){ return false; } N = body_->numJoints(); if(baseLink == targetLink){ isBaseLinkFreeMode = true; } if(isBaseLinkFreeMode){ N += 6; } targetJointPath.find(baseLink, targetLink); pinPropertyMap.erase(targetLink); C = 0; constraintWeightsSqrt.clear(); PinPropertyMap::iterator p = pinPropertyMap.begin(); while(p != pinPropertyMap.end()){ Link* link = p->first; PinProperty& property = p->second; if(!property.jointPath.find(baseLink, link)){ return false; } double weightSqrt = sqrt(property.weight); if(property.axes & InverseKinematics::TRANSLATION_3D){ property.p = link->p; for(int i=0; i < 3; i++){ constraintWeightsSqrt.push_back(weightSqrt); } C += 3; } if(property.axes & InverseKinematics::ROTATION_3D){ property.R = link->R; for(int i=0; i < 3; i++){ constraintWeightsSqrt.push_back(weightSqrt); } C += 3; } p++; } dq.resize(N); y.resize(N); dr_p.resize(M); J.resize(M, N); Jinv.resize(N, M); J.setZero(); jointConstraints.clear(); Jaux.resize(C, N); Jauxinv.resize(N, C); S.resize(C, N); Sinv.resize(N, C); Jaux.setZero(); dPaux.resize(C); int JJsize; if(C <= N && M <= N){ JJsize = C > M ? C : M; } else { JJsize = N; } JJ.resize(JJsize, JJsize); JJ2.resize(JJsize, JJsize); JJinv.resize(JJsize, JJsize); W.resize(N, N); pivots.resize(C); fkTraverse.find(baseLink, true, true); return true; } bool PinDragIK::calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R) { return impl->calcInverseKinematics(end_p, end_R); } bool PinDragIKImpl::calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R) { for(int i=0; i < NJ; i++){ q_org[i] = body_->joint(i)->q; } if(isBaseLinkFreeMode){ base_p_org = baseLink->p; base_R_org = baseLink->R; } for(PinPropertyMap::iterator p = pinPropertyMap.begin(); p != pinPropertyMap.end(); p++){ Link* link = p->first; PinProperty& property = p->second; property.prevStep_p = link->p; property.prevStep_R = link->R; } IKStepResult result = (C > 0) ? PINS_CONVERGED : PINS_NOT_CONVERGED; int i; for(i=0; i < maxIteration; i++){ const Vector3 dp = end_p - targetLink->p; const Vector3 omega = targetLink->R * omegaFromRot(targetLink->R.transpose() * end_R); if((dp.squaredNorm() + omega.squaredNorm()) < ikErrorSqrThresh && result == PINS_CONVERGED){ break; } result = calcOneStep(dp, omega); if(result == ERROR){ for(int i=0; i < NJ; i++){ body_->joint(i)->q = q_org[i]; } if(isBaseLinkFreeMode){ baseLink->p = base_p_org; baseLink->R = base_R_org; } fkTraverse.calcForwardKinematics(); break; } } return (result != ERROR); } PinDragIKImpl::IKStepResult PinDragIKImpl::calcOneStep(const Vector3& v, const Vector3& omega) { //　make Jacobian matrix for the target link int axes = InverseKinematics::TRANSLATION_3D; if(isTargetAttitudeEnabled){ axes |= InverseKinematics::ROTATION_3D; } setJacobianForOnePath(J, 0, targetJointPath, axes); if(isBaseLinkFreeMode){ setJacobianForFreeRoot(J, 0, targetJointPath, axes); } bool isOk; if(N >= M){ isOk = makePseudoInverseType2(M, N, J, Jinv, JJ, JJinv, qWeights, minValidDet); } else { isOk = makePseudoInverseType1(M, N, J, Jinv, JJ, JJinv, minValidDet); } if(!isOk){ return ERROR; } // dq0 = J# dr_p dr_p[0] = v[0]; dr_p[1] = v[1]; dr_p[2] = v[2]; if(isTargetAttitudeEnabled){ dr_p[3] = omega[0]; dr_p[4] = omega[1]; dr_p[5] = omega[2]; } dq.noalias() = Jinv * dr_p; if(C > 0){ solveConstraints(); } double maxq = 0.0; for(int i=0; i < N; ++i){ maxq = std::max(dq[i], maxq); } double thresh = 0.1; if(maxq > thresh){ dq *= (thresh / maxq); } for(int i=0; i < NJ; i++){ body_->joint(i)->q += dq[i]; } if(isBaseLinkFreeMode){ base_p_org = baseLink->p; base_R_org = baseLink->R; for(int i=0; i < 3; i++){ baseLink->p[i] += dq[NJ+i]; } baseLink->R = rotFromRpy(dq[NJ+3], dq[NJ+4], dq[NJ+5]) * baseLink->R; Matrix3& R = baseLink->R; Matrix3::ColXpr x = R.col(0); Matrix3::ColXpr y = R.col(1); Matrix3::ColXpr z = R.col(2); x.normalize(); z = x.cross(y).normalized(); y = z.cross(x); } fkTraverse.calcForwardKinematics(); double maxErrorSqr = 0.0; for(PinPropertyMap::iterator p = pinPropertyMap.begin(); p != pinPropertyMap.end(); p++){ Link* link = p->first; PinProperty& property = p->second; double errsqr = 0.0; if(property.axes & InverseKinematics::TRANSLATION_3D){ const Vector3 dp = property.prevStep_p - link->p; errsqr += dq.squaredNorm(); property.prevStep_p = link->p; } if(property.axes & InverseKinematics::ROTATION_3D){ const Vector3 omega = omegaFromRot(link->R.transpose() * property.prevStep_R); errsqr += omega.squaredNorm(); property.prevStep_R = link->R; } maxErrorSqr = std::max(errsqr, maxErrorSqr); } return (maxErrorSqr < ikErrorSqrThresh) ? PINS_CONVERGED : PINS_NOT_CONVERGED; } void PinDragIKImpl::solveConstraints() { // W = (E - J# J) (size N x N) W.noalias() = MatrixXd::Identity(N, N) - Jinv * J; // normalize W for weighted theta /* for(int i=0; i < N; i++){ double norm2 = 0.0; for(int j=0; j < N; j++){ double a = W(j,i); norm2 += qWeights[j] * a * a; } double norm = sqrt(norm2); if(norm > 0.0001){ for(int j=0; j < N; j++){ W(j,i) /= norm; } } } */ addPinConstraints(); if(isJointRangeConstraintsEnabled){ addJointRangeConstraints(); } // deltaPaux = dPaux - dPaux0, (dPaux0 = Jaux dq0) dPaux.noalias() -= Jaux * dq; VectorXd& deltaPaux = dPaux; S.noalias() = Jaux * W; // normalize S and deltaPaux for weighted targets (weights of constraned positions) /* for(int i=0; i < C; i++){ double w = constraintWeightsSqrt[i]; for(int j=0; j < N; j++){ S(i,j) *= w; } deltaPaux[i] *= w; } */ if(SOLVE_CONSTRAINTS_BY_SR_INVERSE){ makeSRInverseMatrix(C, N, S, Sinv, JJ, JJ2, JJinv, pivots, srk0, srw0); y.noalias() = Sinv * deltaPaux; } else if(SOLVE_CONSTRAINTS_BY_SVD){ y = Eigen::JacobiSVD(S, Eigen::ComputeThinU | Eigen::ComputeThinV).solve(deltaPaux); } // dq = dq0 + W y dq.noalias() += W * y; } void PinDragIKImpl::setJacobianForOnePath(MatrixXd& J, int row, JointPath& jointPath, int axes) { int col; const int n = jointPath.numJoints(); if(n > 0){ Link* target = jointPath.endLink(); for(int i=0; i < n; i++){ Link* link = jointPath.joint(i); col = link->jointId; Vector3 omega(link->R * link->a); if(!jointPath.isJointDownward(i)){ omega = -omega; } int r = row; if(axes & InverseKinematics::TRANSLATION_3D){ const Vector3 dp = omega.cross(target->p - link->p); J(r++, col) = dp(0); J(r++, col) = dp(1); J(r++, col) = dp(2); } if(axes & InverseKinematics::ROTATION_3D){ J(r++, col) = omega(0); J(r++, col) = omega(1); J(r++, col) = omega(2); } } } } void PinDragIKImpl::setJacobianForFreeRoot(MatrixXd& J, int row, JointPath& jointPath, int axes) { Link* target; if(jointPath.numJoints() > 0){ target = jointPath.endLink(); } else { target = baseLink; } int col = NJ; if(axes & InverseKinematics::TRANSLATION_3D){ Vector3 omega = Vector3::Zero(); for(int i=0; i < 3; i++){ omega[i] = 1.0; const Vector3 dp = omega.cross(target->p - baseLink->p); for(int j=0; j < 3; j++){ if(j == i){ J(row + j, col + i) = 1.0; } J(row + j, col + i + 3) = dp(j); } omega[i] = 0.0; } row += 3; } if(axes & InverseKinematics::ROTATION_3D){ for(int i=0; i < 3; i++){ J(row + i, col + i + 3) = 1.0; } } } void PinDragIKImpl::addPinConstraints() { int row = 0; for(PinPropertyMap::iterator p = pinPropertyMap.begin(); p != pinPropertyMap.end(); p++){ Link* link = p->first; PinProperty& property = p->second; setJacobianForOnePath(Jaux, row, property.jointPath, property.axes); if(isBaseLinkFreeMode){ setJacobianForFreeRoot(Jaux, row, property.jointPath, property.axes); } if(property.axes & InverseKinematics::TRANSLATION_3D){ const Vector3 dp = property.p - link->p; dPaux[row++] = dp[0]; dPaux[row++] = dp[1]; dPaux[row++] = dp[2]; } if(property.axes & InverseKinematics::ROTATION_3D){ const Vector3 omega = link->R * omegaFromRot(link->R.transpose() * property.R); dPaux[row++] = omega[0]; dPaux[row++] = omega[1]; dPaux[row++] = omega[2]; } } } void PinDragIKImpl::addJointRangeConstraints() { jointConstraints.clear(); for(int i=0; i < NJ; ++i){ Link* link = body_->joint(i); if(link->q < link->llimit){ jointConstraints.push_back(JointConstrain(i, link->llimit - link->q)); } else if(link->q > link->ulimit){ jointConstraints.push_back(JointConstrain(i, link->ulimit - link->q)); } } const int C2 = jointConstraints.size(); Jaux.conservativeResize(C + C2, N); dPaux.conservativeResize(C + C2); S.resize(C + C2, N); for(int i=0; i < C2; ++i){ int r = C + i; for(int j=0; j < N; ++j){ Jaux(r, j) = 0.0; } JointConstrain& limit = jointConstraints[i]; Jaux(r, limit.jointId) = 1.0; dPaux[r] = limit.dq; } } namespace { /** \param pivots row exchange index in LU decomposition (size = n) */ double calcLU(int n, MatrixXd& a, std::vector& pivots) { double det = 1.0; std::vector weight(n); // get the max element and a scaling value in each row double v, max; for(int k = 0; k < n; k++) { pivots[k] = k; max = 0.0; for(int j = 0; j < n; j++) { v = fabs(a(k,j)); if (v > max) max = v; } if(max == 0.0) return 0.0; // Factorization is impossible ! weight[k] = 1.0 / max; } for(int k = 0; k < n; k++) { // get the next pivot row max = -1; int maxrow; for(int i = k; i < n; i++) { int ix = pivots[i]; v = fabs(a(ix,k)) * weight[ix]; if (v > max){ max = v; maxrow = i; } } int ik = pivots[maxrow]; if (maxrow != k) { pivots[maxrow] = pivots[k]; pivots[k] = ik; det = -det; } double d = a(ik,k); det *= d; if(d == 0) return det; for(int i = k + 1; i < n; i++) { int ix = pivots[i]; double t = (a(ix,k) /= d); for (int j = k + 1; j < n; j++){ a(ix,j) -= t * a(ik,j); } } } return det; } /** Solve Ax = b for x */ void solveByLU(int n, MatrixXd& a, std::vector& pivots, const MatrixXd::ColXpr& x, const VectorXd& b) { int ix; double t; for(int i = 0; i < n; i++) { ix = pivots[i]; t = b(ix); for (int j = 0; j < i; j++){ t -= a(ix, j) * x(j); } const_cast(x)(i) = t; } for(int i = n - 1; i >= 0; i--) { t = x(i); ix = pivots[i]; for (int j = i + 1; j < n; j++){ t -= a(ix, j) * x(j); } const_cast(x)(i) = t / a(ix, i); } } bool makeInverseMatrix(int n, MatrixXd& org, MatrixXd& inv, double minValidDet) { std::vector pivots(n); VectorXd unitVector(n); unitVector.setZero(); double det = calcLU(n, org, pivots); if(det > minValidDet || det < -minValidDet){ for(int i=0; i < n; i++){ unitVector[i] = 1.0; solveByLU(n, org, pivots, inv.col(i), unitVector); unitVector[i] = 0.0; } return true; } else { return false; } } // N < M bool makePseudoInverseType1 (int m, int n, const MatrixXd& J, MatrixXd& Jinv, MatrixXd& JJ, MatrixXd& JJinv, double minValidDet) { // JJ = J^T * J JJ.noalias() = J.transpose() * J; // Jinv = (J^T * J)^-1 * J^T if(makeInverseMatrix(n, JJ, JJinv, minValidDet)){ Jinv.noalias() = JJinv * J.transpose(); return true; } return false; } // N > M bool makePseudoInverseType2 (int m, int n, const MatrixXd& J, MatrixXd& Jinv, MatrixXd& JJ, MatrixXd& JJinv, const VectorXd& weights, double minValidDet) { // JJ = J * W^-1 * J^T for(int i=0; i < m; i++){ for(int j=0; j < m; j++){ JJ(i, j) = 0.0; for(int k=0; k < n; k++){ JJ(i,j) += J(i,k) * (J(j,k) / weights(k)); } } } // Jinv = W^-1 * J^T * (J * W^-1 * J^T)^-1 if(makeInverseMatrix(m, JJ, JJinv, minValidDet)){ for(int i=0; i < n; i++){ for(int j=0; j < m; j++){ Jinv(i,j) = 0.0; for(int k=0; k < m; k++){ Jinv(i,j) += J(k,i) * JJinv(k,j); } Jinv(i,j) /= weights(i); } } return true; } return false; } // calculate J^T(J J^T + kI)^-1 bool makeSRInverseMatrix (int m, int n, const MatrixXd& J, MatrixXd& Jinv, MatrixXd& JJ, MatrixXd& JJ2, MatrixXd& JJinv, std::vector& pivots, double srk0, double srw0) { // JJ = J J^T JJ.noalias() = J * J.transpose(); JJ2 = JJ; double det = calcLU(m, JJ2, pivots); double w = sqrt(det); double k; if(w < srw0){ double a = 1.0 - (w / srw0); k = srk0 * a * a; static int counter = 0; cout << "srk0 enabled(" << counter++ << ")" << endl; } else { k = 0.0; } // JJ' = JJ + kI for(int i=0; i < m; i++){ JJ(i,i) += k; } // J^T JJ'^-1 if(makeInverseMatrix(m, JJ, JJinv, 0.0)){ Jinv.noalias() = J.transpose() * JJinv; return true; } return false; } } choreonoid-1.1.0+dfsg/src/Body/PinDragIK.h000066400000000000000000000030711207742442300201740ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_PIN_DRAG_IK_H_INCLUDED #define CNOID_BODY_PIN_DRAG_IK_H_INCLUDED #include "Body.h" #include "InverseKinematics.h" #include #include "exportdecl.h" namespace cnoid { class PinDragIKImpl; class CNOID_EXPORT PinDragIK : public InverseKinematics { public: PinDragIK(BodyPtr body); ~PinDragIK(); BodyPtr body() const; void setBaseLink(Link* baseLink); void setFreeRootWeight(double translation, double rotation); void setTargetLink(Link* targetLink, bool isAttitudeEnabled = false); void setJointWeight(int jointId, double weight); void setPin(Link* link, InverseKinematics::AxisSet axes = InverseKinematics::TRANSLATION_3D, double weight = 1.0); InverseKinematics::AxisSet pinAxes(Link* link); void clearPins(); int numPinnedLinks(); virtual void setIKErrorThresh(double e); virtual bool hasAnalyticalIK(); virtual InverseKinematics::AxisSet targetAxes() const; void setSRInverseParameters(double k0, double w0); void enableJointRangeConstraints(bool on); /** this must be called before the initial calcInverseKinematics() call after settings have been changed. */ bool initialize(); virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R); private: PinDragIKImpl* impl; }; typedef boost::shared_ptr PinDragIKptr; } #endif choreonoid-1.1.0+dfsg/src/Body/PoseProvider.h000066400000000000000000000016161207742442300210500ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_BODY_POSE_PROVIDER_H_INCLUDED #define CNOID_BODY_POSE_PROVIDER_H_INCLUDED #include "Body.h" #include #include #include namespace cnoid { class PoseProvider { public: virtual ~PoseProvider() { }; virtual Body* body() const = 0; virtual double beginningTime() const = 0; virtual double endingTime() const = 0; virtual bool seek(double time) = 0; virtual bool seek(double time, int waistLinkIndex, const Vector3& waistTranslation) = 0; virtual int baseLinkIndex() const = 0; virtual bool getBaseLinkPosition(Vector3& out_p, Matrix3& out_R) const = 0; virtual void getJointPositions(std::vector< boost::optional >& out_q) const = 0; virtual boost::optional zmp() const = 0; }; } #endif choreonoid-1.1.0+dfsg/src/Body/PoseProviderToBodyMotionConverter.cpp000066400000000000000000000076371207742442300256130ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "PoseProviderToBodyMotionConverter.h" #include "Link.h" #include "LinkPath.h" #include "Sensor.h" #include "BodyMotion.h" #include "PoseProvider.h" using namespace std; using namespace boost; using namespace cnoid; PoseProviderToBodyMotionConverter::PoseProviderToBodyMotionConverter() { setFullTimeRange(); allLinkPositionOutputMode = true; } void PoseProviderToBodyMotionConverter::setTimeRange(double lower, double upper) { lowerTime = std::max(0.0, lower); upperTime = std::max(lowerTime, upper); } void PoseProviderToBodyMotionConverter::setFullTimeRange() { lowerTime = 0.0; upperTime = std::numeric_limits::max(); } void PoseProviderToBodyMotionConverter::setAllLinkPositionOutput(bool on) { allLinkPositionOutputMode = on; } bool PoseProviderToBodyMotionConverter::convert(BodyPtr body, PoseProvider* provider, BodyMotion& motion) { const double frameRate = motion.frameRate(); const int beginningFrame = static_cast(frameRate * std::max(provider->beginningTime(), lowerTime)); const int endingFrame = static_cast(frameRate * std::min(provider->endingTime(), upperTime)); const int numJoints = body->numJoints(); const int numLinksToPut = (allLinkPositionOutputMode ? body->numLinks() : 1); motion.setDimension(endingFrame + 1, numJoints, numLinksToPut, true); MultiValueSeq& qseq = *motion.jointPosSeq(); MultiAffine3Seq& pseq = *motion.linkPosSeq(); Vector3Seq& relZmpSeq = *motion.relativeZmpSeq(); bool isZmpValid = false; Link* rootLink = body->rootLink(); Link* baseLink = rootLink; shared_ptr fkTraverse; if(allLinkPositionOutputMode){ fkTraverse.reset(new LinkTraverse(baseLink, true, true)); } else { fkTraverse.reset(new LinkPath(baseLink, rootLink)); } // store the original state vector orgq(numJoints); for(int i=0; i < numJoints; ++i){ orgq[i] = body->joint(i)->q; } Affine3 orgp; orgp.translation() = rootLink->p; orgp.linear() = rootLink->R; std::vector< boost::optional > jointPositions(numJoints); for(int frame = beginningFrame; frame <= endingFrame; ++frame){ provider->seek(frame / frameRate); const int baseLinkIndex = provider->baseLinkIndex(); if(baseLinkIndex >= 0){ if(baseLinkIndex != baseLink->index){ baseLink = body->link(baseLinkIndex); if(allLinkPositionOutputMode){ fkTraverse->find(baseLink, true, true); } else { static_pointer_cast(fkTraverse)->find(baseLink, rootLink); } } provider->getBaseLinkPosition(baseLink->p, baseLink->R); } MultiValueSeq::View qs = qseq.frame(frame); provider->getJointPositions(jointPositions); for(int i=0; i < numJoints; ++i){ const optional& q = jointPositions[i]; qs[i] = q ? *q : 0.0; body->joint(i)->q = qs[i]; } if(allLinkPositionOutputMode || baseLink != rootLink){ fkTraverse->calcForwardKinematics(); } for(int i=0; i < numLinksToPut; ++i){ Affine3& p = pseq.at(frame, i); Link* link = body->link(i); p.translation() = link->p; p.linear() = link->R; } optional zmp = provider->zmp(); if(zmp){ relZmpSeq[frame].noalias() = rootLink->R.transpose() * (*zmp - rootLink->p); isZmpValid = true; } } if(!isZmpValid){ //bodyMotionItem->clearRelativeZmpSeq(); } // restore the original state for(int i=0; i < numJoints; ++i){ body->joint(i)->q = orgq[i]; } rootLink->p = orgp.translation(); rootLink->R = orgp.linear(); body->calcForwardKinematics(); return true; } choreonoid-1.1.0+dfsg/src/Body/PoseProviderToBodyMotionConverter.h000066400000000000000000000013561207742442300252500ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_POSE_PROVIDER_TO_BODY_MOTION_CONVERTER_H_INCLUDED #define CNOID_BODY_POSE_PROVIDER_TO_BODY_MOTION_CONVERTER_H_INCLUDED #include "Body.h" #include "exportdecl.h" namespace cnoid { class BodyMotion; class PoseProvider; class CNOID_EXPORT PoseProviderToBodyMotionConverter { public: PoseProviderToBodyMotionConverter(); void setTimeRange(double lower, double upper); void setFullTimeRange(); void setAllLinkPositionOutput(bool on); bool convert(BodyPtr body, PoseProvider* provider, BodyMotion& motion); private: double lowerTime; double upperTime; bool allLinkPositionOutputMode; }; } #endif choreonoid-1.1.0+dfsg/src/Body/Sensor.cpp000066400000000000000000000042351207742442300202330ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "Sensor.h" #include using namespace cnoid; Sensor::Sensor() { type = COMMON; } Sensor* Sensor::create(int type) { Sensor* sensor; switch(type){ case FORCE: sensor = new ForceSensor(); break; case RATE_GYRO: sensor = new RateGyroSensor(); break; case ACCELERATION: sensor = new AccelSensor(); break; case RANGE: sensor = new RangeSensor(); break; case PRESSURE: case PHOTO_INTERRUPTER: case VISION: case TORQUE: sensor = new Sensor(); break; default: sensor = 0; } return sensor; } Sensor:: ~Sensor() { } void Sensor::operator=(const Sensor& org) { name = org.name; type = org.type; id = org.id; localR = org.localR; localPos = org.localPos; } void Sensor::destroy(Sensor* sensor) { delete sensor; } void Sensor::clear() { } void Sensor::putInformation(std::ostream &os) { os << "name = " << name << ", id = " << id << "\n"; os << "localAttitude = " << localR << ", localPos = " << localPos << std::endl; } ForceSensor::ForceSensor() { type = FORCE; } void ForceSensor::clear() { f.setZero(); tau.setZero(); } void ForceSensor::putInformation(std::ostream& os) { os << "Force Sensor\n"; Sensor::putInformation(os); os << "f = " << f << "tau = " << tau << std::endl; } RateGyroSensor::RateGyroSensor() { type = RATE_GYRO; } void RateGyroSensor::clear() { w.setZero(); } void RateGyroSensor::putInformation(std::ostream& os) { os << "Gyro\n"; Sensor::putInformation(os); os << "omega = " << w << std::endl; } AccelSensor::AccelSensor() { type = ACCELERATION; clear(); } void AccelSensor::clear() { dv << 0.0, 0.0, 9.8; } void AccelSensor::putInformation(std::ostream& os) { os << "Acceleration Sensor\n"; Sensor::putInformation(os); os << "dv = " << dv << std::endl; } RangeSensor::RangeSensor() { type = RANGE; scanAngle = PI; scanStep = 0.1; scanRate = 10; maxDistance = 10; nextUpdateTime = 0; } choreonoid-1.1.0+dfsg/src/Body/Sensor.h000066400000000000000000000043621207742442300177010ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_SENSOR_H_INCLUDED #define CNOID_BODY_SENSOR_H_INCLUDED #include #include #include #include #include "exportdecl.h" namespace cnoid { class Link; class CNOID_EXPORT Sensor { public: enum SensorType { COMMON = 0, FORCE, RATE_GYRO, ACCELERATION, PRESSURE, PHOTO_INTERRUPTER, VISION, TORQUE, RANGE, NUM_SENSOR_TYPES }; static const int TYPE = COMMON; Sensor(); virtual ~Sensor(); static Sensor* create(int type); static void destroy(Sensor* sensor); virtual void operator=(const Sensor& org); virtual void clear(); std::string name; int type; int id; Link* link; Matrix3 localR; Vector3 localPos; virtual void putInformation(std::ostream& os); }; class CNOID_EXPORT ForceSensor : public Sensor { public: static const int TYPE = FORCE; ForceSensor(); Vector3 f; Vector3 tau; virtual void clear(); virtual void putInformation(std::ostream& os); }; class CNOID_EXPORT RateGyroSensor : public Sensor { public: static const int TYPE = RATE_GYRO; RateGyroSensor(); Vector3 w; virtual void clear(); virtual void putInformation(std::ostream& os); }; class CNOID_EXPORT AccelSensor : public Sensor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW static const int TYPE = ACCELERATION; AccelSensor(); Vector3 dv; virtual void clear(); virtual void putInformation(std::ostream& os); // The following members are used in the ForwardDynamics class Vector2 x[3]; bool isFirstUpdate; }; class CNOID_EXPORT RangeSensor : public Sensor { public: static const int TYPE = RANGE; RangeSensor(); double scanAngle, scanStep, scanRate, maxDistance; std::vector distances; double nextUpdateTime; }; }; #endif choreonoid-1.1.0+dfsg/src/Body/World.cpp000066400000000000000000000104041207742442300200440ustar00rootroot00000000000000/** \author Shin'ichiro Nakaoka */ #include "World.h" #include "Link.h" #include "ForwardDynamicsABM.h" #include "ForwardDynamicsCBM.h" #include #include using namespace std; using namespace cnoid; static const double DEFAULT_GRAVITY_ACCELERATION = 9.80665; static const bool debugMode = false; WorldBase::WorldBase() { currentTime_ = 0.0; timeStep_ = 0.005; g << 0.0, 0.0, DEFAULT_GRAVITY_ACCELERATION; isEulerMethod =false; sensorsAreEnabled = false; numRegisteredLinkPairs = 0; } WorldBase::~WorldBase() { } int WorldBase::bodyIndex(const std::string& name) { NameToIndexMap::iterator p = nameToBodyIndexMap.find(name); return (p != nameToBodyIndexMap.end()) ? p->second : -1; } BodyPtr WorldBase::body(int index) { if(index < 0 || (int)bodyInfoArray.size() <= index) return NULL; return bodyInfoArray[index].body; } BodyPtr WorldBase::body(const std::string& name) { int idx = bodyIndex(name); if(idx < 0 || (int)bodyInfoArray.size() <= idx) return NULL; return bodyInfoArray[idx].body; } void WorldBase::setTimeStep(double ts) { timeStep_ = ts; } void WorldBase::setCurrentTime(double time) { currentTime_ = time; } void WorldBase::setGravityAcceleration(const Vector3& g) { this->g = g; } void WorldBase::enableSensors(bool on) { sensorsAreEnabled = on; } void WorldBase::initialize() { const int n = bodyInfoArray.size(); for(int i=0; i < n; ++i){ BodyInfo& info = bodyInfoArray[i]; BodyPtr body = info.body; bool hasHighGainModeJoints = false; int nL = body->numLinks(); for(int j=0; j < nL; ++j){ if(body->link(j)->isHighGainMode){ hasHighGainModeJoints = true; break; } } if(hasHighGainModeJoints){ info.forwardDynamics.reset(new ForwardDynamicsMM(body)); } else { info.forwardDynamics.reset(new ForwardDynamicsABM(body)); } if(isEulerMethod){ info.forwardDynamics->setEulerMethod(); } else { info.forwardDynamics->setRungeKuttaMethod(); } info.forwardDynamics->setGravityAcceleration(g); info.forwardDynamics->setTimeStep(timeStep_); info.forwardDynamics->enableSensors(sensorsAreEnabled); info.forwardDynamics->initialize(); } } void WorldBase::calcNextState() { if(debugMode){ cout << "World current time = " << currentTime_ << endl; } const int n = bodyInfoArray.size(); for(int i=0; i < n; ++i){ BodyInfo& info = bodyInfoArray[i]; info.forwardDynamics->calcNextState(); } currentTime_ += timeStep_; } int WorldBase::addBody(BodyPtr body) { if(!body->name().empty()){ nameToBodyIndexMap[body->name()] = bodyInfoArray.size(); } BodyInfo info; info.body = body; bodyInfoArray.push_back(info); return bodyInfoArray.size() - 1; } void WorldBase::clearBodies() { nameToBodyIndexMap.clear(); bodyInfoArray.clear(); } void WorldBase::clearCollisionPairs() { linkPairKeyToIndexMap.clear(); numRegisteredLinkPairs = 0; } void WorldBase::setEulerMethod() { isEulerMethod = true; } void WorldBase::setRungeKuttaMethod() { isEulerMethod = false; } std::pair WorldBase::getIndexOfLinkPairs(Link* link1, Link* link2) { int index = -1; bool isRegistered = false; if(link1 != link2){ LinkPairKey linkPair; if(link1 < link2){ linkPair.link1 = link1; linkPair.link2 = link2; } else { linkPair.link1 = link2; linkPair.link2 = link1; } LinkPairKeyToIndexMap::iterator p = linkPairKeyToIndexMap.find(linkPair); if(p != linkPairKeyToIndexMap.end()){ index = p->second; isRegistered = true; } else { index = numRegisteredLinkPairs++; linkPairKeyToIndexMap[linkPair] = index; } } return std::make_pair(index, isRegistered); } bool WorldBase::LinkPairKey::operator<(const LinkPairKey& pair2) const { if(link1 < pair2.link1){ return true; } else if(link1 == pair2.link1){ return (link2 < pair2.link2); } else { return false; } } choreonoid-1.1.0+dfsg/src/Body/World.h000066400000000000000000000117641207742442300175230ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODY_WORLD_H_INCLUDED #define CNOID_BODY_WORLD_H_INCLUDED #include "Body.h" #include "ForwardDynamics.h" #include #include "exportdecl.h" namespace cnoid { class Link; class CNOID_EXPORT WorldBase { public: WorldBase(); virtual ~WorldBase(); /** @brief get the number of bodies in this world @return the number of bodies */ inline int numBodies() { return bodyInfoArray.size(); } /** @brief get body by index @param index of the body @return body */ BodyPtr body(int index); /** @brief get body by name @param name of the body @return body */ BodyPtr body(const std::string& name); /** @brief get forward dynamics computation method for body @param index index of the body @return forward dynamics computation method */ inline ForwardDynamicsPtr forwardDynamics(int index) { return bodyInfoArray[index].forwardDynamics; } /** @brief get index of body by name @param name of the body @return index of the body */ int bodyIndex(const std::string& name); /** @brief add body to this world @param body @return index of the body @note This must be called before initialize() is called. */ int addBody(BodyPtr body); /** @brief clear bodies in this world */ void clearBodies(); /** @brief clear collision pairs */ void clearCollisionPairs(); /** @brief set time step @param dt time step[s] */ void setTimeStep(double dt); /** @brief get time step @return time step[s] */ double timeStep(void) const { return timeStep_; } /** @brief set current time @param tm current time[s] */ void setCurrentTime(double tm); /** @brief get current time @return current time[s] */ double currentTime(void) const { return currentTime_; } /** @brief set gravity acceleration @param g gravity acceleration[m/s^2] */ void setGravityAcceleration(const Vector3& g); /** @brief get gravity acceleration @return gravity accleration */ inline const Vector3& gravityAcceleration() { return g; } /** @brief enable/disable sensor simulation @param on true to enable, false to disable @note This must be called before initialize() is called. */ void enableSensors(bool on); /** @brief choose euler method for integration */ void setEulerMethod(); /** @brief choose runge-kutta method for integration */ void setRungeKuttaMethod(); /** @brief initialize this world. This must be called after all bodies are registered. */ virtual void initialize(); /** @brief compute forward dynamics and update current state */ virtual void calcNextState(); /** @brief get index of link pairs @param link1 link1 @param link2 link2 @return pair of index and flag. The flag is true if the pair was already registered, false othewise. */ std::pair getIndexOfLinkPairs(Link* link1, Link* link2); protected: double currentTime_; double timeStep_; struct BodyInfo { BodyPtr body; ForwardDynamicsPtr forwardDynamics; }; std::vector bodyInfoArray; bool sensorsAreEnabled; private: typedef std::map NameToIndexMap; NameToIndexMap nameToBodyIndexMap; typedef std::map BodyToIndexMap; BodyToIndexMap bodyToIndexMap; struct LinkPairKey { Link* link1; Link* link2; bool operator<(const LinkPairKey& pair2) const; }; typedef std::map LinkPairKeyToIndexMap; LinkPairKeyToIndexMap linkPairKeyToIndexMap; int numRegisteredLinkPairs; Vector3 g; bool isEulerMethod; // Euler or Runge Kutta ? }; template class World : public WorldBase { public: TConstraintForceSolver constraintForceSolver; World() : constraintForceSolver(*this) { } virtual void initialize() { WorldBase::initialize(); constraintForceSolver.initialize(); } virtual void calcNextState(){ constraintForceSolver.solve(); WorldBase::calcNextState(); } }; }; #endif choreonoid-1.1.0+dfsg/src/Body/exportdecl.h000066400000000000000000000004461207742442300206000ustar00rootroot00000000000000 #ifdef CNOID_EXPORT #undef CNOID_EXPORT #endif #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) # ifdef CnoidBody_EXPORTS # define CNOID_EXPORT __declspec(dllexport) # else # define CNOID_EXPORT __declspec(dllimport) # endif #else # define CNOID_EXPORT #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/000077500000000000000000000000001207742442300174315ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyBar.cpp000066400000000000000000000231621207742442300214630ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "BodyBar.h" #include #include #include #include #include #include "gettext.h" using namespace boost; using namespace cnoid; BodyBar* BodyBar::instance() { static BodyBar* instance = new BodyBar(); return instance; } BodyBar::BodyBar() : ToolBar("BodyBar"), mes(*MessageView::mainInstance()) { addButton(QIcon(":/Body/icons/storepose.png"), _("Memory the current pose")) ->sigClicked().connect(bind(&BodyBar::onCopyButtonClicked, this)); addButton(QIcon(":/Body/icons/restorepose.png"), _("Recall the memorized pose")) ->sigClicked().connect(bind(&BodyBar::onPasteButtonClicked, this)); addButton(QIcon(":/Body/icons/origin.png"), _("Move the selected bodies to the origin")) ->sigClicked().connect(bind(&BodyBar::onOriginButtonClicked, this)); addButton(QIcon(":/Body/icons/initialpose.png"), _("Set the preset initial pose to the selected bodies")) ->sigClicked().connect(bind(&BodyBar::onPoseButtonClicked, this, BodyItem::INITIAL_POSE)); addButton(QIcon(":/Body/icons/stdpose.png"), _("Set the preset standard pose to the selected bodies")) ->sigClicked().connect(bind(&BodyBar::onPoseButtonClicked, this, BodyItem::STANDARD_POSE)); addSeparator(); addButton(QIcon(":/Body/icons/right-to-left.png"), _("Copy the right side pose to the left side")) ->sigClicked().connect(bind(&BodyBar::onSymmetricCopyButtonClicked, this, 1, false)); addButton(QIcon(":/Body/icons/flip.png"), _("Mirror copy")) ->sigClicked().connect(bind(&BodyBar::onSymmetricCopyButtonClicked, this, 0, true)); addButton(QIcon(":/Body/icons/left-to-right.png"), _("Copy the left side pose to the right side")) ->sigClicked().connect(bind(&BodyBar::onSymmetricCopyButtonClicked, this, 0, false)); addSeparator(); addButton(QIcon(":/Body/icons/center-cm.png"), _("Move the center of mass to the position where its projection corresponds to the support feet cener"))-> sigClicked().connect(bind(&BodyBar::moveCM, this, BodyItem::HOME_COP)); addButton(QIcon(":/Body/icons/zmp-to-cm.png"), _("Move the center of mass to fit its projection to ZMP"))-> sigClicked().connect(bind(&BodyBar::moveCM, this, BodyItem::ZMP)); addSeparator(); addButton(QIcon(":/Body/icons/cm-to-zmp.png"), _("Set ZMP to the projection of the center of mass")) ->sigClicked().connect(bind(&BodyBar::setZmp, this, BodyItem::CM_PROJECTION)); addButton(QIcon(":/Body/icons/right-zmp"), _("Set ZMP under the right foot")) ->sigClicked().connect(bind(&BodyBar::setZmp, this, BodyItem::RIGHT_HOME_COP)); addButton(QIcon(":/Body/icons/center-zmp.png"), _("Set ZMP at the center of the feet")) ->sigClicked().connect(bind(&BodyBar::setZmp, this, BodyItem::HOME_COP)); addButton(QIcon(":/Body/icons/left-zmp.png"), _("Set ZMP under the left foot")) ->sigClicked().connect(bind(&BodyBar::setZmp, this, BodyItem::LEFT_HOME_COP)); addSeparator(); addButton(QIcon(":/Body/icons/stancelength.png"), _("Adjust the width between the feet")) ->sigClicked().connect(bind(&BodyBar::setStance, this)); stanceWidthSpin = new DoubleSpinBox(); stanceWidthSpin->setAlignment(Qt::AlignCenter); stanceWidthSpin->setToolTip(_("Width between the feet [m]")); stanceWidthSpin->setDecimals(4); stanceWidthSpin->setRange(0.0001, 9.9999); stanceWidthSpin->setSingleStep(0.001); stanceWidthSpin->setValue(0.15); addWidget(stanceWidthSpin); ItemTreeView::mainInstance()->sigSelectionChanged().connect( bind(&BodyBar::onItemSelectionChanged, this, _1)); } BodyBar::~BodyBar() { connectionOfCurrentBodyItemDetachedFromRoot.disconnect(); } /** \todo ItemTreeView::sigSelectionChanged() should be emitted after the final selection state has been determined. */ bool BodyBar::makeSingleSelection(BodyItemPtr bodyItem) { ItemTreeView* tree = ItemTreeView::mainInstance()->mainInstance(); ItemList prevSelected = selectedBodyItems_; for(size_t i=0; i < prevSelected.size(); ++i){ BodyItem* item = prevSelected[i]; if(item != bodyItem && tree->isItemSelected(item)){ tree->selectItem(item, false); } } bool selected = tree->isItemSelected(bodyItem); if(!selected){ selected = tree->selectItem(bodyItem, true); } return selected; } void BodyBar::onItemSelectionChanged(const ItemList& bodyItems) { bool selectedBodyItemsChanged = false; if(selectedBodyItems_ != bodyItems){ selectedBodyItems_ = bodyItems; selectedBodyItemsChanged = true; } BodyItemPtr firstItem = bodyItems.toSingle(); if(firstItem && firstItem != currentBodyItem_){ currentBodyItem_ = firstItem; connectionOfCurrentBodyItemDetachedFromRoot.disconnect(); connectionOfCurrentBodyItemDetachedFromRoot = currentBodyItem_->sigDetachedFromRoot().connect( bind(&BodyBar::onBodyItemDetachedFromRoot, this)); sigCurrentBodyItemChanged_(currentBodyItem_.get()); } if(selectedBodyItemsChanged){ sigBodyItemSelectionChanged_(selectedBodyItems_); } targetBodyItems.clear(); if(selectedBodyItems_.empty()){ if(currentBodyItem_){ targetBodyItems.push_back(currentBodyItem_); } } else { targetBodyItems = selectedBodyItems_; } } void BodyBar::onCopyButtonClicked() { if(currentBodyItem_){ currentBodyItem_->copyKinematicState(); } } void BodyBar::onPasteButtonClicked() { for(size_t i=0; i < targetBodyItems.size(); ++i){ targetBodyItems[i]->pasteKinematicState(); } } void BodyBar::onBodyItemDetachedFromRoot() { currentBodyItem_ = 0; connectionOfCurrentBodyItemDetachedFromRoot.disconnect(); sigCurrentBodyItemChanged_(0); } void BodyBar::onOriginButtonClicked() { for(size_t i=0; i < targetBodyItems.size(); ++i){ targetBodyItems[i]->moveToOrigin(); } } void BodyBar::onPoseButtonClicked(BodyItem::PresetPoseID id) { for(size_t i=0; i < targetBodyItems.size(); ++i){ targetBodyItems[i]->setPresetPose(id); } } void BodyBar::onSymmetricCopyButtonClicked(int direction, bool doMirrorCopy) { for(size_t i=0; i < targetBodyItems.size(); ++i){ const YamlSequence& slinks = *targetBodyItems[i]->body()->info()->findSequence("symmetricJoints"); if(slinks.isValid() && !slinks.empty()){ targetBodyItems[i]->beginKinematicStateEdit(); int from = direction; int to = 1 - direction; BodyPtr body = targetBodyItems[i]->body(); for(int j=0; j < slinks.size(); ++j){ const YamlSequence& linkPair = *slinks[j].toSequence(); if(linkPair.size() == 1 && doMirrorCopy){ Link* link = body->link(linkPair[0].toString()); if(link){ link->q = -link->q; } } else if(linkPair.size() >= 2){ Link* link1 = body->link(linkPair[from].toString()); Link* link2 = body->link(linkPair[to].toString()); if(link1 && link2){ double sign = 1.0; if(linkPair.size() >= 3){ sign = linkPair[2].toDouble(); } if(doMirrorCopy){ double q1 = link1->q; link1->q = sign * link2->q; link2->q = sign * q1; } else { link2->q = sign * link1->q; } } } } targetBodyItems[i]->notifyKinematicStateChange(true); targetBodyItems[i]->acceptKinematicStateEdit(); } } } void BodyBar::moveCM(BodyItem::PositionType position) { for(size_t i=0; i < targetBodyItems.size(); ++i){ BodyItem* bodyItem = targetBodyItems[i]; Vector3 c = bodyItem->centerOfMass(); optional p = bodyItem->getParticularPosition(position); if(p){ c[0] = (*p)[0]; c[1] = (*p)[1]; } if(!bodyItem->doLegIkToMoveCm(c, true)){ static format f(_("The center of mass of %1% cannt be moved to the target position\n")); mes.notify(str(f % bodyItem->name())); } } } void BodyBar::setZmp(BodyItem::PositionType position) { for(size_t i=0; i < targetBodyItems.size(); ++i){ optional p = targetBodyItems[i]->getParticularPosition(position); if(p){ targetBodyItems[i]->editZmp(*p); } } } void BodyBar::setStance() { for(size_t i=0; i < targetBodyItems.size(); ++i){ targetBodyItems[i]->setStance(stanceWidthSpin->value()); } } bool BodyBar::storeState(Archive& archive) { if(currentBodyItem_){ archive.writeItemId("current", currentBodyItem_); } archive.write("stanceWidth", stanceWidthSpin->value()); return true; } bool BodyBar::restoreState(const Archive& archive) { stanceWidthSpin->setValue(archive.get("stanceWidth", stanceWidthSpin->value())); if(!currentBodyItem_){ currentBodyItem_ = archive.findItem("current"); if(currentBodyItem_){ if(targetBodyItems.empty()){ targetBodyItems.push_back(currentBodyItem_); } sigCurrentBodyItemChanged_(currentBodyItem_.get()); } } return true; } choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyBar.h000066400000000000000000000042611207742442300211270ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_BODY_BAR_H_INCLUDED #define CNOID_BODYPLUGIN_BODY_BAR_H_INCLUDED #include "BodyItem.h" #include #include #include #include "exportdecl.h" namespace cnoid { class MessageView; class CNOID_EXPORT BodyBar : public ToolBar, public boost::signals::trackable { public: static BodyBar* instance(); virtual ~BodyBar(); boost::signal& selectedBodyItems)>& sigBodyItemSelectionChanged() { return sigBodyItemSelectionChanged_; } SignalProxy< boost::signal > sigCurrentBodyItemChanged() { return sigCurrentBodyItemChanged_; } inline const ItemList& selectedBodyItems() { return selectedBodyItems_; } inline BodyItem* currentBodyItem() { return currentBodyItem_.get(); } bool makeSingleSelection(BodyItemPtr bodyItem); protected: virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); private: BodyBar(); MessageView& mes; BodyItemPtr currentBodyItem_; ItemList selectedBodyItems_; ItemList targetBodyItems; DoubleSpinBox* stanceWidthSpin; boost::signals::connection connectionOfCurrentBodyItemDetachedFromRoot; boost::signal& selectedBodyItems)> sigBodyItemSelectionChanged_; boost::signal sigCurrentBodyItemChanged_; void onItemSelectionChanged(const ItemList& bodyItems); void onBodyItemDetachedFromRoot(); void onCopyButtonClicked(); void onPasteButtonClicked(); void onOriginButtonClicked(); void onPoseButtonClicked(BodyItem::PresetPoseID id); void onSymmetricCopyButtonClicked(int direction, bool doMirrorCopy); void moveCM(BodyItem::PositionType position); void setZmp(BodyItem::PositionType position); void setStance(); }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyItem.cpp000066400000000000000000000556011207742442300216600ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka @todo Eliminate the dependency on WorldItem and KinematicsBar */ #include "BodyItem.h" #include "WorldItem.h" #include "KinematicsBar.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; BodyItem::KinematicState kinematicStateCopy; /// \todo move this to hrpUtil ? inline double radian(double deg) { return (3.14159265358979 * deg / 180.0); } bool loadBodyItem(BodyItem* item, const std::string& filename, std::ostream& os) { bool loaded = false; if(item->loadModelFile(filename)){ if(item->name().empty()){ item->setName(item->body()->modelName()); } loaded = true; } else { os << item->errorMessage(); } return loaded; } void onSigOptionsParsed(boost::program_options::variables_map& variables) { if(variables.count("hrpmodel")){ vector modelFileNames = variables["hrpmodel"].as< vector >(); for(size_t i=0; i < modelFileNames.size(); ++i){ BodyItemPtr item(new BodyItem()); if(item->load(modelFileNames[i], "OpenHRP-VRML-MODEL")){ RootItem::mainInstance()->addChildItem(item); } } } } } void cnoid::initializeBodyItem(ExtensionManager& ext) { ext.itemManager().registerClass(N_("BodyItem")); ext.itemManager().addLoader( _("OpenHRP model file"), "OpenHRP-VRML-MODEL", "wrl;yaml", bind(loadBodyItem, _1, _2, _3)); ext.optionManager().addOption("hrpmodel", program_options::value< vector >(), "load an OpenHRP model file"); ext.optionManager().sigOptionsParsed().connect(onSigOptionsParsed); kinematicStateCopy.p.setZero(); kinematicStateCopy.R.setIdentity(); kinematicStateCopy.zmp.setZero(); } BodyItem::BodyItem() : body_(new Body()), sigKinematicStateChanged_(bind(&BodyItem::emitSigKinematicStateChanged, this)), sigKinematicStateEdited_(bind(&BodyItem::emitSigKinematicStateEdited, this)), updateSelfCollisionsCaller(bind(&BodyItem::updateSelfCollisions, this, false), IDLE_PRIORITY_NORMAL) { isSelfCollisionDetectionEnabled_ = true; init(); } BodyItem::BodyItem(const BodyItem& org) : Item(org), body_(org.body_->duplicate()), modelNodeSet_(org.modelNodeSet_), modelFilePath_(org.modelFilePath_), sigKinematicStateChanged_(bind(&BodyItem::emitSigKinematicStateChanged, this)), updateSelfCollisionsCaller(bind(&BodyItem::updateSelfCollisions, this, false), IDLE_PRIORITY_NORMAL) { isSelfCollisionDetectionEnabled_ = org.isSelfCollisionDetectionEnabled_; init(); setCurrentBaseLink(body_->link(org.currentBaseLink()->index)); } void BodyItem::init() { kinematicsBar = KinematicsBar::instance(); isFkRequested = isVelFkRequested = isAccFkRequested = false; currentHistoryIndex = 0; isCurrentKinematicStateInHistory = false; needToAppendKinematicStateToHistory = false; isCallingSlotsOnKinematicStateEdited = false; isSelfCollisionUpdateNeeded = false; isColdetModelPositionUpdateNeeded = false; initBody(); sigPositionChanged().connect(bind(&BodyItem::onPositionChanged, this)); } BodyItem::~BodyItem() { } void BodyItem::initBody() { setCurrentBaseLink(body_->rootLink()); if(pinDragIK_){ pinDragIK_.reset(); } zmp_.setZero(); int n = body_->numLinks(); worldColdetPairsOfLink_.resize(n); worldCollisionLinkBitSet.resize(n); selfCollisionLinkBitSet.resize(n); updateSelfColdetPairs(); } void BodyItem::onPositionChanged() { worldItem_ = findOwnerItem(); if(!worldItem_){ for(size_t i=0; i < worldColdetPairsOfLink_.size(); ++i){ worldColdetPairsOfLink_[i].clear(); } } } bool BodyItem::loadModelFile(const std::string& filename) { errorMessage_.clear(); BodyLoader bodyLoader; MessageView::mainInstance()->beginStdioRedirect(); BodyPtr newBody = bodyLoader.loadModelFile(filename, true, true, true); MessageView::mainInstance()->endStdioRedirect(); if(!newBody){ errorMessage_ = bodyLoader.errorMessage(); modelNodeSet_.reset(); modelFilePath_.clear(); } else { body_ = newBody; body_->setName(name()); modelNodeSet_ = bodyLoader.modelNodeSet(); modelFilePath_ = filename; } initBody(); return (newBody); } void BodyItem::setName(const std::string& name) { if(body_){ body_->setName(name); } Item::setName(name); } void BodyItem::setCurrentBaseLink(Link* link) { if(link != currentBaseLink_){ if(link){ fkTraverse.find(link, true, true); } else { fkTraverse.find(body_->rootLink()); } } currentBaseLink_ = link; } /** Forward kinematics from the current base link is done. */ void BodyItem::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) { fkTraverse.calcForwardKinematics(calcVelocity, calcAcceleration); } void BodyItem::copyKinematicState() { storeKinematicState(kinematicStateCopy); } void BodyItem::pasteKinematicState() { restoreKinematicState(kinematicStateCopy); notifyKinematicStateChange(false); } void BodyItem::storeKinematicState(KinematicState& state) { const int n = body_->numJoints(); state.q.resize(n); for(int i=0; i < n; ++i){ state.q[i] = body_->joint(i)->q; } state.p = body_->rootLink()->p; state.R = body_->rootLink()->R; state.zmp = zmp_; } /** @return false if the restored state is same as the current state */ bool BodyItem::restoreKinematicState(const KinematicState& state) { bool modified = false; int n = std::min(static_cast(state.q.size()), body_->numJoints()); for(int i=0; i < n; ++i){ double& q = body_->joint(i)->q; if(q != state.q[i]){ q = state.q[i]; modified = true; } } Link* root = body_->rootLink(); if(!modified){ modified = (root->p != state.p || root->R != state.R || zmp_ != state.zmp); } if(modified){ root->p = state.p; root->R = state.R; zmp_ = state.zmp; body_->calcForwardKinematics(); } return modified; } void BodyItem::beginKinematicStateEdit() { if(TRACE_FUNCTIONS){ cout << "BodyItem::beginKinematicStateEdit()" << endl; } if(!isCurrentKinematicStateInHistory){ appendKinematicStateToHistory(); } } void BodyItem::acceptKinematicStateEdit() { if(TRACE_FUNCTIONS){ cout << "BodyItem::acceptKinematicStateEdit()" << endl; } //appendKinematicStateToHistory(); needToAppendKinematicStateToHistory = true; sigKinematicStateEdited_.request(); } void BodyItem::appendKinematicStateToHistory() { if(TRACE_FUNCTIONS){ cout << "BodyItem::appendKinematicStateToHistory()" << endl; } KinematicStatePtr state(new KinematicState); storeKinematicState(*state); if(kinematicStateHistory.empty() || (currentHistoryIndex == kinematicStateHistory.size() - 1)){ kinematicStateHistory.push_back(state); currentHistoryIndex = kinematicStateHistory.size() - 1; } else { ++currentHistoryIndex; kinematicStateHistory.resize(currentHistoryIndex + 1); kinematicStateHistory[currentHistoryIndex] = state; } if(kinematicStateHistory.size() > 20){ kinematicStateHistory.pop_front(); currentHistoryIndex--; } isCurrentKinematicStateInHistory = true; } bool BodyItem::undoKinematicState() { if(TRACE_FUNCTIONS){ cout << "BodyItem::undoKinematicState()" << endl; } bool done = false; bool modified = false; if(!isCurrentKinematicStateInHistory){ if(currentHistoryIndex < kinematicStateHistory.size()){ done = true; modified = restoreKinematicState(*kinematicStateHistory[currentHistoryIndex]); } } else { if(currentHistoryIndex > 0){ done = true; modified = restoreKinematicState(*kinematicStateHistory[--currentHistoryIndex]); } } if(done){ if(modified){ notifyKinematicStateChange(false); isCurrentKinematicStateInHistory = true; sigKinematicStateEdited_.request(); } else { isCurrentKinematicStateInHistory = true; done = undoKinematicState(); } } return done; } bool BodyItem::redoKinematicState() { if(TRACE_FUNCTIONS){ cout << "BodyItem::redoKinematicState()" << endl; } if(currentHistoryIndex + 1 < kinematicStateHistory.size()){ restoreKinematicState(*kinematicStateHistory[++currentHistoryIndex]); notifyKinematicStateChange(false); isCurrentKinematicStateInHistory = true; sigKinematicStateEdited_.request(); return true; } return false; } PinDragIKptr BodyItem::pinDragIK() { if(!pinDragIK_){ pinDragIK_.reset(new PinDragIK(body_)); } return pinDragIK_; } InverseKinematicsPtr BodyItem::getCurrentIK(Link* targetLink) { InverseKinematicsPtr ik; if(KinematicsBar::instance()->mode() == KinematicsBar::AUTO_MODE){ ik = body_->getDefaultIK(targetLink); } if(!ik){ pinDragIK(); // create if not created if(pinDragIK_->numPinnedLinks() > 0 || !currentBaseLink_){ pinDragIK_->setTargetLink(targetLink, true); if(pinDragIK_->initialize()){ ik = pinDragIK_; } } } if(!ik){ if(currentBaseLink_){ ik = body_->getJointPath(currentBaseLink_, targetLink); } } return ik; } PenetrationBlockerPtr BodyItem::createPenetrationBlocker(Link* link, bool excludeSelfCollisions) { PenetrationBlockerPtr blocker; if(link->body == body_ && worldItem_){ std::vector& pairs = worldColdetPairsOfLink(link->index); if(!pairs.empty()){ blocker.reset(new PenetrationBlocker(link)); for(size_t i=0; i < pairs.size(); ++i){ Link* oppnentLink = pairs[i]->link(0); if(oppnentLink == link){ oppnentLink = pairs[i]->link(1); } if(excludeSelfCollisions){ if(oppnentLink->body == body_){ continue; } } blocker->addOpponentLink(oppnentLink); } blocker->setDepth(kinematicsBar->penetrationBlockDepth()); } } return blocker; } void BodyItem::moveToOrigin() { beginKinematicStateEdit(); Vector3 p; Matrix3 R; body_->getDefaultRootPosition(p, R); Link* rootLink = body_->rootLink(); rootLink->p = p; rootLink->R = R; body_->calcForwardKinematics(); notifyKinematicStateChange(false); acceptKinematicStateEdit(); } void BodyItem::setPresetPose(PresetPoseID id) { int jointIndex = 0; beginKinematicStateEdit(); if(id == STANDARD_POSE){ const YamlSequence& pose = *body_->info()->findSequence("standardPose"); if(pose.isValid()){ const int n = std::min(pose.size(), body_->numJoints()); while(jointIndex < n){ body_->joint(jointIndex)->q = radian(pose[jointIndex].toDouble()); jointIndex++; } } } const int n = body_->numJoints(); while(jointIndex < n){ body_->joint(jointIndex++)->q = 0.0; } fkTraverse.calcForwardKinematics(); notifyKinematicStateChange(false); acceptKinematicStateEdit(); } const Vector3& BodyItem::centerOfMass() { if(!updateFlags.test(UF_CM)){ body_->calcCM(); updateFlags.set(UF_CM); } return body_->lastCM(); } /** \todo use getDefaultIK() if the kinematics bar is in the AUTO mode. */ bool BodyItem::doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor) { bool result = false; LeggedBody* legged = dynamic_cast(body_.get()); if(legged){ KinematicState orgKinematicState; storeKinematicState(orgKinematicState); beginKinematicStateEdit(); result = legged->doLegIkToMoveCm(c, onlyProjectionToFloor); if(result){ notifyKinematicStateChange(); acceptKinematicStateEdit(); updateFlags.set(UF_CM); } else { restoreKinematicState(orgKinematicState); } } return result; } bool BodyItem::setStance(double width) { bool result = false; LeggedBody* legged = dynamic_cast(body_.get()); if(legged){ KinematicState orgKinematicState; storeKinematicState(orgKinematicState); beginKinematicStateEdit(); result = legged->setStance(width, currentBaseLink_); if(result){ notifyKinematicStateChange(); acceptKinematicStateEdit(); } else { restoreKinematicState(orgKinematicState); } } return result; } boost::optional BodyItem::getParticularPosition(PositionType position) { boost::optional pos; if(position == ZMP){ pos = zmp_; } else { if(position == CM_PROJECTION){ pos = centerOfMass(); } else { LeggedBody* legged = dynamic_cast(body_.get()); if(legged){ if(position == HOME_COP){ pos = legged->homeCopOfSoles(); } else if(position == RIGHT_HOME_COP || position == LEFT_HOME_COP) { if(legged->numFeet() == 2){ pos = legged->homeCopOfSole((position == RIGHT_HOME_COP) ? 0 : 1); } } } } if(pos){ (*pos).z() = 0.0; } } return pos; } void BodyItem::editZmp(const Vector3& zmp) { beginKinematicStateEdit(); setZmp(zmp); notifyKinematicStateChange(false); acceptKinematicStateEdit(); } void BodyItem::notifyKinematicStateChange(bool requestFK, bool requestVelFK, bool requestAccFK) { if(!isCallingSlotsOnKinematicStateEdited){ isCurrentKinematicStateInHistory = false; } if(requestFK){ isFkRequested |= requestFK; isVelFkRequested |= requestVelFK; isAccFkRequested |= requestAccFK; } updateFlags.reset(); if(isSelfCollisionDetectionEnabled_){ isSelfCollisionUpdateNeeded = true; } isColdetModelPositionUpdateNeeded = true; sigKinematicStateChanged_.request(); } void BodyItem::notifyKinematicStateChange (boost::signals::connection& connectionToBlock, bool requestFK, bool requestVelFK, bool requestAccFK) { sigKinematicStateChanged_.requestBlocking(connectionToBlock); notifyKinematicStateChange(requestFK, requestVelFK, requestAccFK); } void BodyItem::emitSigKinematicStateChanged() { if(isFkRequested){ fkTraverse.calcForwardKinematics(isVelFkRequested, isAccFkRequested); isFkRequested = isVelFkRequested = isAccFkRequested = false; } sigKinematicStateChanged_.signal()(); if(isSelfCollisionUpdateNeeded){ updateSelfCollisionsCaller.setPriority(kinematicsBar->collisionDetectionPriority()); updateSelfCollisionsCaller.request(); } if(needToAppendKinematicStateToHistory){ appendKinematicStateToHistory(); needToAppendKinematicStateToHistory = false; } } void BodyItem::emitSigKinematicStateEdited() { isCallingSlotsOnKinematicStateEdited = true; sigKinematicStateEdited_.signal()(); isCallingSlotsOnKinematicStateEdited = false; if(!sigKinematicStateEdited_.isBeingRequested() && needToAppendKinematicStateToHistory){ appendKinematicStateToHistory(); needToAppendKinematicStateToHistory = false; } } void BodyItem::enableSelfCollisionDetection(bool on) { if(isSelfCollisionDetectionEnabled_ && !on){ isSelfCollisionDetectionEnabled_ = false; clearSelfCollisions(); notifyUpdate(); } else if(!isSelfCollisionDetectionEnabled_ && on){ isSelfCollisionDetectionEnabled_ = true; updateSelfColdetPairs(); notifyUpdate(); } } void BodyItem::updateSelfColdetPairs() { selfColdetPairs.clear(); int n = body_->numLinks(); int excludeTreeDepth = 1; dynamic_bitset<> exclusions(n); const YamlMapping& selfCollisionInfo = *body_->info()->findMapping("selfCollisionDetection"); if(selfCollisionInfo.isValid()){ excludeTreeDepth = selfCollisionInfo.get("excludeTreeDepth", 1); const YamlSequence& excludeLinks = *selfCollisionInfo.findSequence("excludeLinks"); for(int i=0; i < excludeLinks.size(); ++i){ Link* link = body_->link(excludeLinks[i].toString()); if(link){ exclusions[link->index] = true; } } } for(int i=0; i < n; ++i){ Link* link1 = body_->link(i); if(!exclusions[link1->index]){ for(int j=i+1; j < n; ++j){ Link* link2 = body_->link(j); if(!exclusions[link2->index]){ bool skip = false; Link* parent1 = link1; Link* parent2 = link2; for(int k=0; k < excludeTreeDepth; ++k){ if(parent1){ parent1 = parent1->parent; } if(parent2){ parent2 = parent2->parent; } if(!parent1 && !parent2){ break; } if(parent1 == link2 || parent2 == link1){ skip = true; break; } } if(!skip){ selfColdetPairs.push_back(new ColdetLinkPair(link1, link2)); } } } } } if(isSelfCollisionDetectionEnabled_){ updateSelfCollisions(true); } else { clearSelfCollisions(); } } void BodyItem::updateColdetModelPositions(bool force) { if(isColdetModelPositionUpdateNeeded || force){ const int n = body_->numLinks(); for(int i=0; i < n; ++i){ body_->link(i)->updateColdetModelPosition(); } isColdetModelPositionUpdateNeeded = false; } } /** @ret true if a colliding link set changes */ bool BodyItem::updateSelfCollisions(bool force) { if(isSelfCollisionUpdateNeeded || force){ bool collisionLinkSetChanged = false; if(!selfColdetPairs.empty()){ updateColdetModelPositions(); selfCollisionLinkBitSet.reset(); for(size_t i=0; i < selfColdetPairs.size(); ++i){ ColdetLinkPair& linkPair = *selfColdetPairs[i]; bool prevEmpty = linkPair.collisions().empty(); bool empty = linkPair.detectCollisions().empty(); if(prevEmpty != empty){ collisionLinkSetChanged = true; } if(!empty){ selfCollisionLinkBitSet.set(linkPair.link(0)->index); selfCollisionLinkBitSet.set(linkPair.link(1)->index); } } if(collisionLinkSetChanged){ sigSelfCollisionLinkSetChanged_(); } sigSelfCollisionsUpdated_(); } isSelfCollisionUpdateNeeded = false; return collisionLinkSetChanged; } return false; } ItemPtr BodyItem::doDuplicate() const { return new BodyItem(*this); } void BodyItem::clearSelfCollisions() { selfCollisionLinkBitSet.reset(); sigSelfCollisionLinkSetChanged_(); sigSelfCollisionsUpdated_(); } bool BodyItem::onSelfCollisionDetectionPropertyChanged(bool on) { enableSelfCollisionDetection(on); return true; } void BodyItem::doPutProperties(PutPropertyFunction& putProperty) { putProperty(_("Model name"), body_->modelName()); putProperty(_("Num links"), body_->numLinks()); putProperty(_("Num joints"), body_->numJoints()); putProperty(_("Root link"), body_->rootLink()->name()); putProperty(_("Base link"), currentBaseLink_ ? currentBaseLink_->name() : "none"); putProperty(_("Mass"), body_->totalMass()); putProperty(_("Static model ?"), body_->isStaticModel()); putProperty(_("Model file"), filesystem::path(modelFilePath_).leaf()); putProperty(_("Self-collision"), isSelfCollisionDetectionEnabled_, (bind(&BodyItem::onSelfCollisionDetectionPropertyChanged, this, _1))); // This does not call the function, why ? // (bind(&BodyItem::enableSelfCollisionDetection, this, _1), constant(true))); } bool BodyItem::store(Archive& archive) { archive.setDoubleFormat("% .6f"); archive.writeRelocatablePath("modelFile", modelFilePath_); archive.write("currentBaseLink", (currentBaseLink_ ? currentBaseLink_->name() : ""), YAML_DOUBLE_QUOTED); write(archive, "rootPosition", body_->rootLink()->p); write(archive, "rootAttitude", body_->rootLink()->R); YamlSequence& qs = *archive.createFlowStyleSequence("jointPositions"); int n = body_->numJoints(); for(int i=0; i < n; ++i){ qs.append(body_->joint(i)->q, 10, n); } archive.write("selfCollisionDetection", isSelfCollisionDetectionEnabled_); return true; } bool BodyItem::restore(const Archive& archive) { bool restored = false; string modelFile; if(archive.readRelocatablePath("modelFile", modelFile)){ restored = modelFile.empty() || load(modelFile); } if(restored){ read(archive, "rootPosition", body_->rootLink()->p); read(archive, "rootAttitude", body_->rootLink()->R); const YamlSequence& qs = *archive.findSequence("jointPositions"); if(qs.isValid()){ for(int i=0; i < qs.size(); ++i){ body_->joint(i)->q = qs[i].toDouble(); } } body_->calcForwardKinematics(); setCurrentBaseLink(body_->link(archive.get("currentBaseLink", ""))); notifyKinematicStateChange(); enableSelfCollisionDetection(archive.get("selfCollisionDetection", true)); } return restored; } choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyItem.h000066400000000000000000000176521207742442300213310ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_BODY_ITEM_H_INCLUDED #define CNOID_BODYPLUGIN_BODY_ITEM_H_INCLUDED #include #include #include #include #include #include #include #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class BodyItem; typedef boost::intrusive_ptr BodyItemPtr; class WorldItem; class PinDragIK; typedef boost::shared_ptr PinDragIKptr; class PenetrationBlocker; typedef boost::shared_ptr PenetrationBlockerPtr; class LinkGroup; class KinematicsBar; void initializeBodyItem(ExtensionManager& ext); class CNOID_EXPORT BodyItem : public Item { public: BodyItem(); BodyItem(const BodyItem& org); virtual ~BodyItem(); void init(); bool loadModelFile(const std::string& filename); virtual void setName(const std::string& name); inline Body* body() { return body_.get(); } inline ModelNodeSetPtr modelNodeSet() { return modelNodeSet_; } const std::string& errorMessage() { return errorMessage_; } enum PresetPoseID { INITIAL_POSE, STANDARD_POSE }; void moveToOrigin(); void setPresetPose(PresetPoseID id); inline Link* currentBaseLink() const { return currentBaseLink_; } void setCurrentBaseLink(Link* link); void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false); void copyKinematicState(); void pasteKinematicState(); struct KinematicState { private: std::vector q; Vector3 p; Matrix3 R; Vector3 zmp; friend class BodyItem; friend void cnoid::initializeBodyItem(ExtensionManager& ext); }; void storeKinematicState(KinematicState& state); bool restoreKinematicState(const KinematicState& state); // for undo, redo operations void beginKinematicStateEdit(); void acceptKinematicStateEdit(); bool undoKinematicState(); bool redoKinematicState(); PinDragIKptr pinDragIK(); InverseKinematicsPtr getCurrentIK(Link* targetLink); PenetrationBlockerPtr createPenetrationBlocker(Link* link, bool excludeSelfCollisions = false); /** @if jp ロボットã®é–¢ç¯€è§’ã€é–¢ç¯€è§’速度ã€rootä½ç½®ãƒ»å§¿å‹¢ãªã©ã®ã€Œé‹å‹•å­¦çš„ã€çŠ¶æ…‹ã«å¤‰æ›´ãŒç”Ÿã˜ãŸã¨ã㫠発行ã•ã‚Œã‚‹ã‚·ã‚°ãƒŠãƒ«ã€‚ Item::sigUpdated() ã¯ãƒ¢ãƒ‡ãƒ«è‡ªä½“ãŒå¤‰ã‚ã£ãŸå ´åˆã¨ã—ã€ãã¡ã‚‰ã¨ã¯åŒºåˆ¥ã—ã¦ä½¿ã†ã€‚ @endif */ SignalProxy< boost::signal > sigKinematicStateChanged() { return sigKinematicStateChanged_.signal(); } void notifyKinematicStateChange( bool requestFK = false, bool requestVelFK = false, bool requestAccFK = false); void notifyKinematicStateChange( boost::signals::connection& connectionToBlock, bool requestFK = false, bool requestVelFK = false, bool requestAccFK = false); SignalProxy< boost::signal > sigKinematicStateEdited() { return sigKinematicStateEdited_.signal(); } /** @ret WorldItem that contains this body item if exists. */ WorldItem* worldItem() { return worldItem_; } void updateColdetModelPositions(bool force = false); void enableSelfCollisionDetection(bool on); bool isSelfCollisionDetectionEnabled() { return isSelfCollisionDetectionEnabled_; } bool updateSelfCollisions(bool force = false); void clearSelfCollisions(); std::vector selfColdetPairs; boost::dynamic_bitset<> selfCollisionLinkBitSet; SignalProxy< boost::signal > sigSelfCollisionsUpdated() { return sigSelfCollisionsUpdated_; } SignalProxy< boost::signal > sigSelfCollisionLinkSetChanged() { return sigSelfCollisionLinkSetChanged_; } std::vector& worldColdetPairsOfLink(int linkIndex) { return worldColdetPairsOfLink_[linkIndex]; } const std::vector& worldColdetPairsOfLink(int linkIndex) const { return worldColdetPairsOfLink_[linkIndex]; } boost::dynamic_bitset<> worldCollisionLinkBitSet; SignalProxy< boost::signal > sigWorldCollisionsUpdated() { return sigWorldCollisionsUpdated_; } SignalProxy< boost::signal > sigWorldCollisionLinkSetChanged() { return sigWorldCollisionLinkSetChanged_; } void notifyWorldCollisionLinkSetChange() { sigWorldCollisionLinkSetChanged_(); } void notifyWorldCollisionUpdate() { sigWorldCollisionsUpdated_(); } const Vector3& centerOfMass(); bool doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor = false); inline const Vector3& zmp() { return zmp_; } void setZmp(const Vector3& zmp) { zmp_ = zmp; } void editZmp(const Vector3& zmp); enum PositionType { CM_PROJECTION, HOME_COP, RIGHT_HOME_COP, LEFT_HOME_COP, ZMP }; boost::optional getParticularPosition(PositionType posType); //void setZmp(ZmpPosition position); bool setStance(double width); const std::string modelFilePath() { return modelFilePath_; } protected: virtual ItemPtr doDuplicate() const; virtual void doPutProperties(PutPropertyFunction& putProperty); virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); private: BodyPtr body_; ModelNodeSetPtr modelNodeSet_; std::string modelFilePath_; std::string errorMessage_; enum { UF_POSITIONS, UF_VELOCITIES, UF_ACCELERATIONS, UF_CM, UF_ZMP, NUM_UPUDATE_FLAGS }; std::bitset updateFlags; typedef boost::shared_ptr KinematicStatePtr; std::deque kinematicStateHistory; size_t currentHistoryIndex; bool isCurrentKinematicStateInHistory; bool needToAppendKinematicStateToHistory; LazySignal< boost::signal > sigKinematicStateChanged_; LazySignal< boost::signal > sigKinematicStateEdited_; LazySignal< boost::signal > sigStateUpdated_; bool isCallingSlotsOnKinematicStateEdited; bool isFkRequested; bool isVelFkRequested; bool isAccFkRequested; Link* currentBaseLink_; LinkTraverse fkTraverse; PinDragIKptr pinDragIK_; Vector3 zmp_; WorldItem* worldItem_; std::vector< std::vector > worldColdetPairsOfLink_; bool isSelfCollisionDetectionEnabled_; bool isSelfCollisionUpdateNeeded; bool isColdetModelPositionUpdateNeeded; KinematicsBar* kinematicsBar; LazyCaller updateSelfCollisionsCaller; boost::signal sigSelfCollisionsUpdated_; boost::signal sigSelfCollisionLinkSetChanged_; boost::signal sigWorldCollisionsUpdated_; boost::signal sigWorldCollisionLinkSetChanged_; void initBody(); void emitSigKinematicStateChanged(); void emitSigKinematicStateEdited(); void appendKinematicStateToHistory(); void updateSelfColdetPairs(); bool onSelfCollisionDetectionPropertyChanged(bool on); void onPositionChanged(); }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyLinkView.cpp000066400000000000000000000633031207742442300225100ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "BodyLinkView.h" #include "BodyBar.h" #include "BodyItem.h" #include "LinkSelectionView.h" #include "WorldItem.h" #include "KinematicsBar.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; static const double sliderResolution = 1000000.0; } namespace cnoid { class BodyLinkViewImpl : public boost::signals::trackable { public: BodyLinkViewImpl(BodyLinkView* self); ~BodyLinkViewImpl(); BodyLinkView* self; QScrollArea scrollArea; QLabel nameLabel; QLabel linkIndexLabel; QLabel jointIdLabel; QLabel jointTypeLabel; QLabel jointAxisLabel; QGroupBox qGroup; DoubleSpinBox qSpin; QLabel qMinLabel; QLabel qMaxLabel; Slider qSlider; QGroupBox dqGroup; QLabel dqLabel; DoubleSpinBox dqMinSpin; DoubleSpinBox dqMaxSpin; DoubleSpinBox xyzSpin[3]; DoubleSpinBox rpySpin[3]; CheckBox attMatrixCheck; QWidget attMatrixBox; QLabel attLabels[3][3]; DoubleSpinBox zmpXyzSpin[3]; CheckBox selfCollisionCheck; QLabel worldCollisionsLabel; QLabel selfCollisionsLabel; BodyItemPtr currentBodyItem; Link* currentLink; WorldItem* currentWorldItem; vector selfColdetPairs; signals::connection connectionToLinkSelectionChanged; signals::connection connectionToKinematicStateChanged; signals::connection connectionToBodyModelUpdated; signals::connection selfCollisionsUpdatedConnection; signals::connection worldColdetPairsUpdatedConnection; signals::connection worldCollisionsUpdatedConnection; ConnectionSet propertyWidgetConnections; ConnectionSet stateWidgetConnections; void setupWidgets(); void onAttMatrixCheckToggled(); void onCurrentBodyItemChanged(BodyItem* bodyItem); void activateCurrentBodyItem(bool on); void update(); void updateLink(); void updateKinematicState(bool blockSignals); void updateColdetPairs(); void updateWorldColdetPairs(); void updateSelfCollisions(); void updateWorldCollisions(); void on_qSpinChanged(double value); void on_qSliderChanged(int value); void on_qChanged(double q); void on_dqLimitChanged(bool isMin); void onXyzChanged(); void onRpyChanged(); void doInverseKinematics(Vector3 p, Matrix3 R); void onZmpXyzChanged(); void onSelfCollisionToggled(bool checked); bool storeState(Archive& archive); bool restoreState(const Archive& archive); }; } namespace { // margins const int M1 = 2; const int M2 = 4; const int M3 = 8; const int M4 = 16; } BodyLinkView::BodyLinkView() { impl = new BodyLinkViewImpl(this); } BodyLinkViewImpl::BodyLinkViewImpl(BodyLinkView* self) : self(self) { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::BodyLinkViewImpl" << endl; } self->setName(N_("Body / Link")); self->setDefaultLayoutArea(View::CENTER); currentWorldItem = 0; currentBodyItem = 0; currentLink = 0; setupWidgets(); BodyBar::instance()->sigCurrentBodyItemChanged().connect( bind(&BodyLinkViewImpl::onCurrentBodyItemChanged, this, _1)); self->sigActivated().connect(bind(&BodyLinkViewImpl::activateCurrentBodyItem, this, true)); self->sigDeactivated().connect(bind(&BodyLinkViewImpl::activateCurrentBodyItem, this, false)); } BodyLinkView::~BodyLinkView() { delete impl; } BodyLinkViewImpl::~BodyLinkViewImpl() { } void BodyLinkViewImpl::setupWidgets() { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::setupWidgets" << endl; } QHBoxLayout* hbox; QVBoxLayout* vbox; QGridLayout* grid; QWidget* topWidget = new QWidget(); QVBoxLayout* topVBox = new QVBoxLayout(); //topVBox->setContentsMargins(4); topWidget->setLayout(topVBox); scrollArea.setFrameShape(QFrame::NoFrame); scrollArea.setWidget(topWidget); QVBoxLayout* baseLayout = new QVBoxLayout(); scrollArea.setWidgetResizable(true); baseLayout->addWidget(&scrollArea); self->setLayout(baseLayout); //nameLabel.setAlignment(Qt::AlignCenter); topVBox->addWidget(&nameLabel, 0, Qt::AlignCenter); topVBox->addSpacing(4); QFrame* frame = new QFrame(); topVBox->addWidget(frame); grid = new QGridLayout(); //grid->setContentsMargins(4); grid->setVerticalSpacing(4); grid->setColumnStretch(1, 1); grid->setColumnStretch(3, 1); grid->addWidget(new QLabel(_("Index:")), 0, 0); grid->addWidget(&linkIndexLabel, 0, 1); grid->addWidget(new QLabel(_("Joint ID:")), 0, 2); grid->addWidget(&jointIdLabel, 0, 3); grid->addWidget(new QLabel(_("Joint Type:")), 1, 0); grid->addWidget(&jointTypeLabel, 1, 1, 1, 3); grid->addWidget(new QLabel(_("Joint Axis:")), 2, 0); grid->addWidget(&jointAxisLabel, 2, 1, 1, 3); frame->setLayout(grid); topVBox->addSpacing(4); qGroup.setAlignment(Qt::AlignHCenter); topVBox->addWidget(&qGroup); vbox = new QVBoxLayout(); //vbox->setContentsMargins(4); qGroup.setLayout(vbox); hbox = new QHBoxLayout(); vbox->addLayout(hbox); hbox->addStretch(); hbox->addWidget(&qMinLabel); qSpin.setAlignment(Qt::AlignCenter); hbox->addWidget(&qSpin); hbox->addWidget(&qMaxLabel); hbox->addStretch(); qSlider.setOrientation(Qt::Horizontal); vbox->addWidget(&qSlider); stateWidgetConnections.add( qSpin.sigValueChanged().connect( bind(&BodyLinkViewImpl::on_qSpinChanged, this, _1))); stateWidgetConnections.add( qSlider.sigValueChanged().connect( bind(&BodyLinkViewImpl::on_qSliderChanged, this, _1))); topVBox->addSpacing(4); dqGroup.setAlignment(Qt::AlignHCenter); topVBox->addWidget(&dqGroup); hbox = new QHBoxLayout(); //hbox->setContentsMargins(4); // min velocity spin hbox->addStretch(); hbox->addWidget(new QLabel(_("min"))); dqMaxSpin.setAlignment(Qt::AlignCenter); hbox->addWidget(&dqMinSpin); // velocity label hbox->addWidget(&dqLabel); // max velocity spin dqMinSpin.setAlignment(Qt::AlignCenter); hbox->addWidget(&dqMaxSpin); hbox->addWidget(new QLabel(_("max"))); hbox->addStretch(); dqGroup.setLayout(hbox); propertyWidgetConnections.add( dqMinSpin.sigValueChanged().connect( bind(&BodyLinkViewImpl::on_dqLimitChanged, this, true))); propertyWidgetConnections.add( dqMaxSpin.sigValueChanged().connect( bind(&BodyLinkViewImpl::on_dqLimitChanged, this, false))); topVBox->addSpacing(4); frame = new QFrame(); vbox = new QVBoxLayout(); //vbox->setContentsMargins(4); vbox->setSpacing(4); frame->setLayout(vbox); topVBox->addWidget(frame); vbox->addWidget(new QLabel(_("Link Position [m],[deg]"))); grid = new QGridLayout(); //grid->setContentsMargins(4); grid->setVerticalSpacing(4); vbox->addLayout(grid); static const char* xyzLabels[] = {"X", "Y", "Z"}; for(int i=0; i < 3; ++i){ grid->addWidget(new QLabel(xyzLabels[i], frame), 0, i, Qt::AlignCenter); grid->addWidget(&xyzSpin[i], 1, i); //xyzSpin[i].set_width_chars(7); xyzSpin[i].setAlignment(Qt::AlignCenter); xyzSpin[i].setDecimals(4); xyzSpin[i].setRange(-99.9999, 99.9999); xyzSpin[i].setSingleStep(0.0001); stateWidgetConnections.add( xyzSpin[i].sigValueChanged().connect( bind(&BodyLinkViewImpl::onXyzChanged, this))); } static const char* rpyLabels[] = {"R", "P", "Y"}; for(int i=0; i < 3; ++i){ grid->addWidget(new QLabel(rpyLabels[i], frame), 2, i, Qt::AlignCenter); grid->addWidget(&rpySpin[i], 3, i); rpySpin[i].setAlignment(Qt::AlignCenter); rpySpin[i].setDecimals(1); rpySpin[i].setRange(-360.0, 360.0); rpySpin[i].setSingleStep(0.1); stateWidgetConnections.add( rpySpin[i].sigValueChanged().connect( bind(&BodyLinkViewImpl::onRpyChanged, this))); } attMatrixCheck.setText(_("Matrix")); attMatrixCheck.sigToggled().connect( bind(&BodyLinkViewImpl::onAttMatrixCheckToggled, this)); vbox->addWidget(&attMatrixCheck, 0, Qt::AlignCenter); grid = new QGridLayout(); grid->setHorizontalSpacing(10); //grid->setContentsMargins(4); grid->setVerticalSpacing(4); hbox = new QHBoxLayout(); attMatrixBox.setLayout(hbox); vbox->addWidget(&attMatrixBox); hbox->addStretch(); hbox->addWidget(new QLabel("R = ")); hbox->addWidget(new VSeparator()); hbox->addLayout(grid); hbox->addWidget(new VSeparator()); hbox->addStretch(); for(int i=0; i < 3; ++i){ for(int j=0; j < 3; ++j){ grid->addWidget(&attLabels[i][j], i, j); attLabels[i][j].setText("0.0"); } } topVBox->addSpacing(4); QGroupBox* group = new QGroupBox(); group->setTitle(_("ZMP [m]")); group->setAlignment(Qt::AlignCenter); grid = new QGridLayout(); //grid->setContentsMargins(4); grid->setVerticalSpacing(2); group->setLayout(grid); for(int i=0; i < 3; ++i){ grid->addWidget(new QLabel(xyzLabels[i], group), 0, i, Qt::AlignCenter); grid->addWidget(&zmpXyzSpin[i], 1, i); //zmpXyzSpin[i].set_width_chars(7); zmpXyzSpin[i].setAlignment(Qt::AlignCenter); zmpXyzSpin[i].setDecimals(4); zmpXyzSpin[i].setRange(-99.9999, 99.9999); zmpXyzSpin[i].setSingleStep(0.0001); stateWidgetConnections.add( zmpXyzSpin[i].sigValueChanged().connect( bind(&BodyLinkViewImpl::onZmpXyzChanged, this))); } topVBox->addWidget(group); topVBox->addSpacing(4); group = new QGroupBox(); group->setTitle(_("Collisions")); group->setAlignment(Qt::AlignCenter); vbox = new QVBoxLayout(); //vbox->setContentsMargins(4); vbox->setSpacing(4); worldCollisionsLabel.setAlignment(Qt::AlignCenter); worldCollisionsLabel.setWordWrap(true); vbox->addWidget(&worldCollisionsLabel); hbox = new QHBoxLayout(); hbox->setSpacing(4); hbox->addWidget(new HSeparator()); selfCollisionCheck.setText(_("Self-Collisions")); propertyWidgetConnections.add( selfCollisionCheck.sigToggled().connect( bind(&BodyLinkViewImpl::onSelfCollisionToggled, this, _1))); hbox->addWidget(&selfCollisionCheck); hbox->addWidget(new HSeparator()); vbox->addLayout(hbox); selfCollisionsLabel.setAlignment(Qt::AlignCenter); selfCollisionsLabel.setWordWrap(true); vbox->addWidget(&selfCollisionsLabel); topVBox->addSpacing(4); group->setLayout(vbox); topVBox->addWidget(group); attMatrixBox.hide(); } void BodyLinkViewImpl::onAttMatrixCheckToggled() { if(attMatrixCheck.isChecked()){ attMatrixBox.show(); updateKinematicState(true); } else { attMatrixBox.hide(); } } void BodyLinkViewImpl::onCurrentBodyItemChanged(BodyItem* bodyItem) { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::onCurrentBodyItemChanged" << endl; } if(bodyItem != currentBodyItem){ activateCurrentBodyItem(false); currentBodyItem = bodyItem; currentLink = 0; activateCurrentBodyItem(true); } } void BodyLinkViewImpl::activateCurrentBodyItem(bool on) { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::activateCurrentBodyItem" << endl; } if(on){ if(!connectionToKinematicStateChanged.connected() && self->isActive() && currentBodyItem){ connectionToLinkSelectionChanged = LinkSelectionView::mainInstance()->sigSelectionChanged(currentBodyItem).connect (bind(&BodyLinkViewImpl::update, this)); connectionToKinematicStateChanged = currentBodyItem->sigKinematicStateChanged().connect( bind(&BodyLinkViewImpl::updateKinematicState, this, true)); connectionToBodyModelUpdated = currentBodyItem->sigUpdated().connect(bind(&BodyLinkViewImpl::update, this)); update(); } } else { connectionToLinkSelectionChanged.disconnect(); connectionToKinematicStateChanged.disconnect(); connectionToBodyModelUpdated.disconnect(); selfCollisionsUpdatedConnection.disconnect(); worldColdetPairsUpdatedConnection.disconnect(); worldCollisionsUpdatedConnection.disconnect(); } } void BodyLinkViewImpl::update() { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::update" << endl; } currentLink = 0; if(!currentBodyItem){ nameLabel.setText(""); return; } propertyWidgetConnections.block(); stateWidgetConnections.block(); BodyPtr body = currentBodyItem->body(); const vector& selectedLinkIndices = LinkSelectionView::mainInstance()->getSelectedLinkIndices(currentBodyItem); if(selectedLinkIndices.empty()){ currentLink = body->rootLink(); } else { currentLink = body->link(selectedLinkIndices.front()); } if(currentLink){ nameLabel.setText(QString("%1 / %2").arg(body->name().c_str()).arg(currentLink->name().c_str())); updateLink(); } else { nameLabel.setText(body->name().c_str()); } selfCollisionCheck.setChecked(currentBodyItem->isSelfCollisionDetectionEnabled()); updateColdetPairs(); updateKinematicState(false); stateWidgetConnections.unblock(); propertyWidgetConnections.unblock(); } void BodyLinkViewImpl::updateLink() { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::updateLink" << endl; } BodyPtr body = currentBodyItem->body(); linkIndexLabel.setText(QString::number(currentLink->index)); jointIdLabel.setText(QString::number(currentLink->jointId)); Vector3 a(currentLink->Rs.transpose() * currentLink->a); jointAxisLabel.setText(QString("(%1 %2 %3)").arg(a[0], 0, 'f', 4).arg(a[1], 0, 'f', 4).arg(a[2], 0, 'f', 4)); double qmin = degree(currentLink->llimit); double qmax = degree(currentLink->ulimit); if(currentLink->jointType == Link::FREE_JOINT){ jointTypeLabel.setText(_("Free")); } else if(currentLink->jointType == Link::FIXED_JOINT){ jointTypeLabel.setText(_("Fixed")); } else if(currentLink->jointType == Link::ROTATIONAL_JOINT){ jointTypeLabel.setText(_("Rotation")); qGroup.setTitle(_("Joint Angle [deg]")); qSpin.setDecimals(1); //qSpin.setRange(qmin, qmax); qSpin.setRange(-360.0, 360.0); // Limit over values should be shown qSpin.setSingleStep(0.1); qMinLabel.setText(QString::number(qmin, 'f', 1)); qMaxLabel.setText(QString::number(qmax, 'f', 1)); qSlider.setRange(qmin * sliderResolution, qmax * sliderResolution); qSlider.setSingleStep(0.1 * sliderResolution); dqGroup.setTitle(_("Joint Velocity [deg/s]")); dqMinSpin.setDecimals(1); dqMinSpin.setRange(-999.9, 0.0); dqMinSpin.setSingleStep(0.1); dqMinSpin.setValue(degree(currentLink->lvlimit)); dqMaxSpin.setDecimals(1); dqMaxSpin.setRange(0.0, 999.9); dqMaxSpin.setSingleStep(0.1); dqMaxSpin.setValue(degree(currentLink->uvlimit)); } else if(currentLink->jointType == Link::SLIDE_JOINT){ jointTypeLabel.setText(_("Slide")); qGroup.setTitle(_("Joint Translation [m]:")); qSpin.setDecimals(4); qSpin.setRange(-9.9999, 9.9999); qSpin.setSingleStep(0.0001); qSlider.setRange(qmin * sliderResolution, qmax * sliderResolution); qSlider.setSingleStep(0.001 * sliderResolution); dqGroup.setTitle(_("Joint Velocity [m/s]")); dqMinSpin.setDecimals(3); dqMinSpin.setRange(-9.999, 9.999); dqMinSpin.setSingleStep(0.001); dqMinSpin.setValue(currentLink->lvlimit); dqMaxSpin.setDecimals(3); dqMaxSpin.setRange(-9.999, 9.999); dqMaxSpin.setSingleStep(0.001); dqMaxSpin.setValue(currentLink->uvlimit); } } void BodyLinkViewImpl::updateKinematicState(bool blockSignals) { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::updateKinematicState" << endl; } if(currentBodyItem){ if(blockSignals){ stateWidgetConnections.block(); } if(currentLink){ if(currentLink->jointType == Link::ROTATIONAL_JOINT){ double q = degree(currentLink->q); qSpin.setValue(q); qSlider.setValue(q * sliderResolution); dqLabel.setText(QString::number(degree(currentLink->dq), 'f', 1)); } else if(currentLink->jointType == Link::SLIDE_JOINT){ qSpin.setValue(currentLink->q); qSlider.setValue(currentLink->q * sliderResolution); dqLabel.setText(QString::number(currentLink->dq, 'f', 1)); } const Matrix3 R = currentLink->attitude(); const Vector3 rpy = rpyFromRot(R); for(int i=0; i < 3; ++i){ xyzSpin[i].setValue(currentLink->p[i]); rpySpin[i].setValue(degree(rpy[i])); } if(attMatrixCheck.isChecked()){ for(int i=0; i < 3; ++i){ for(int j=0; j < 3; ++j){ attLabels[i][j].setText(QString::number(R(i,j), 'f', 6)); } } } } const Vector3& zmp = currentBodyItem->zmp(); for(int i=0; i < 3; ++i){ zmpXyzSpin[i].setValue(zmp[i]); } if(blockSignals){ stateWidgetConnections.unblock(); } } } void BodyLinkViewImpl::updateColdetPairs() { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::updateColdetPairs" << endl; } selfColdetPairs.clear(); selfCollisionsUpdatedConnection.disconnect(); if(currentBodyItem->isSelfCollisionDetectionEnabled() && currentLink){ vector selfPairs = currentBodyItem->selfColdetPairs; for(size_t i=0; i < selfPairs.size(); ++i){ ColdetLinkPairPtr& selfPair = selfPairs[i]; if(selfPair->link(0) == currentLink || selfPair->link(1) == currentLink){ selfColdetPairs.push_back(selfPair); } } if(!selfColdetPairs.empty()){ selfCollisionsUpdatedConnection = currentBodyItem->sigSelfCollisionsUpdated().connect( bind(&BodyLinkViewImpl::updateSelfCollisions, this)); } } updateSelfCollisions(); worldColdetPairsUpdatedConnection.disconnect(); if(currentLink){ currentWorldItem = currentBodyItem->worldItem(); if(currentWorldItem){ worldColdetPairsUpdatedConnection = currentWorldItem->sigColdetPairsUpdated().connect( bind(&BodyLinkViewImpl::updateWorldColdetPairs, this)); } } updateWorldColdetPairs(); } void BodyLinkViewImpl::updateWorldColdetPairs() { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::updateWorldColdetPairs" << endl; } worldCollisionsUpdatedConnection.disconnect(); if(currentWorldItem && currentWorldItem->isCollisionDetectionEnabled() && currentLink){ if(!currentBodyItem->worldColdetPairsOfLink(currentLink->index).empty()){ worldCollisionsUpdatedConnection = currentBodyItem->sigWorldCollisionLinkSetChanged().connect( bind(&BodyLinkViewImpl::updateWorldCollisions, this)); } } updateWorldCollisions(); } void BodyLinkViewImpl::updateSelfCollisions() { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::updateSelfCollisions" << endl; } QString resultText; for(size_t i=0; i < selfColdetPairs.size(); ++i){ ColdetLinkPairPtr& linkPair = selfColdetPairs[i]; const std::vector& cols = linkPair->collisions(); if(!cols.empty()){ Link* oppositeLink; if(linkPair->link(0) == currentLink){ oppositeLink = linkPair->link(1); } else { oppositeLink = linkPair->link(0); } if(!resultText.isEmpty()){ resultText += " "; } resultText += oppositeLink->name().c_str(); } } selfCollisionsLabel.setText(resultText); } void BodyLinkViewImpl::updateWorldCollisions() { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::updateWorldCollisions" << endl; } QString resultText; if(currentLink){ std::vector& coldetPairs = currentBodyItem->worldColdetPairsOfLink(currentLink->index); for(size_t i=0; i < coldetPairs.size(); ++i){ ColdetLinkPairPtr& linkPair = coldetPairs[i]; const std::vector& cols = linkPair->collisions(); if(!cols.empty()){ Link* oppositeLink; if(linkPair->link(0) == currentLink){ oppositeLink = linkPair->link(1); } else { oppositeLink = linkPair->link(0); } if(!resultText.isEmpty()){ resultText += " "; } resultText += oppositeLink->body->name().c_str(); resultText += " / "; resultText += oppositeLink->name().c_str(); } } worldCollisionsLabel.setText(resultText); } } void BodyLinkViewImpl::on_qSpinChanged(double value) { on_qChanged(value); } void BodyLinkViewImpl::on_qSliderChanged(int value) { on_qChanged(value / sliderResolution); } void BodyLinkViewImpl::on_qChanged(double q) { if(currentLink){ if(currentLink->jointType == Link::ROTATIONAL_JOINT){ q = radian(q); } currentLink->q = q; currentBodyItem->notifyKinematicStateChange(true); } } void BodyLinkViewImpl::on_dqLimitChanged(bool isMin) { if(currentLink){ DoubleSpinBox& targetSpin = isMin ? dqMinSpin : dqMaxSpin; double& targetVal = isMin ? currentLink->lvlimit : currentLink->uvlimit; double& oppositeVal = isMin ? currentLink->uvlimit : currentLink->lvlimit; double limit = targetSpin.value(); if(currentLink->jointType == Link::ROTATIONAL_JOINT){ limit = radian(limit); } if(currentLink->uvlimit == -currentLink->lvlimit){ oppositeVal = -limit; } targetVal = limit; currentBodyItem->notifyUpdate(); } } void BodyLinkViewImpl::onXyzChanged() { if(currentLink){ Vector3 p; for(int i=0; i < 3; ++i){ p[i] = xyzSpin[i].value(); } doInverseKinematics(p, currentLink->R); } } void BodyLinkViewImpl::onRpyChanged() { if(currentLink){ Vector3 rpy; for(int i=0; i < 3; ++i){ rpy[i] = radian(rpySpin[i].value()); } doInverseKinematics(currentLink->p, currentLink->calcRfromAttitude(rotFromRpy(rpy))); } } void BodyLinkViewImpl::doInverseKinematics(Vector3 p, Matrix3 R) { InverseKinematicsPtr ik = currentBodyItem->getCurrentIK(currentLink); if(ik){ currentBodyItem->beginKinematicStateEdit(); if(KinematicsBar::instance()->isPenetrationBlockMode()){ PenetrationBlockerPtr blocker = currentBodyItem->createPenetrationBlocker(currentLink, true); if(blocker){ blocker->adjust(p, R, Vector3(p - currentLink->p)); } } if(ik->calcInverseKinematics(p, R)){ currentBodyItem->notifyKinematicStateChange(true); currentBodyItem->acceptKinematicStateEdit(); } } } void BodyLinkViewImpl::onZmpXyzChanged() { if(currentBodyItem){ Vector3 zmp; for(int i=0; i < 3; ++i){ zmp[i] = zmpXyzSpin[i].value(); } currentBodyItem->setZmp(zmp); currentBodyItem->notifyKinematicStateChange(false); } } void BodyLinkViewImpl::onSelfCollisionToggled(bool checked) { if(TRACE_FUNCTIONS){ cout << "BodyLinkViewImpl::onSelfCollisionToggled" << endl; } if(currentBodyItem){ currentBodyItem->enableSelfCollisionDetection(checked); currentBodyItem->notifyUpdate(); } } bool BodyLinkView::storeState(Archive& archive) { return impl->storeState(archive); } bool BodyLinkViewImpl::storeState(Archive& archive) { archive.write("showRotationMatrix", attMatrixCheck.isChecked()); return true; } bool BodyLinkView::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool BodyLinkViewImpl::restoreState(const Archive& archive) { attMatrixCheck.setChecked(archive.get("showRotationMatrix", attMatrixCheck.isChecked())); return true; } choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyLinkView.h000066400000000000000000000007771207742442300221630ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_BODY_LINK_VIEW_H_INCLUDED #define CNOID_BODYPLUGIN_BODY_LINK_VIEW_H_INCLUDED #include namespace cnoid { class BodyLinkViewImpl; class BodyLinkView : public cnoid::View { public: BodyLinkView(); virtual ~BodyLinkView(); private: BodyLinkViewImpl* impl; virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyMotionEngine.cpp000066400000000000000000000130751207742442300233540ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "BodyMotionEngine.h" #include "BodyItem.h" #include "BodyMotionItem.h" #include #include using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; class BodyMotionEngine : public TimeSyncItemEngine { public: BodyItemPtr bodyItem; BodyPtr body; MultiValueSeqPtr qSeq; MultiAffine3SeqPtr positions; Vector3SeqPtr relativeZmpSeq; bool calcForwardKinematics; BodyMotionEngine() { } bool initialize(Item* item) { BodyMotionItem* bodyMotionItem; MultiValueSeqItem* multiValueSeqItem; MultiAffine3SeqItem* multiAffine3SeqItem;; if((bodyMotionItem = dynamic_cast(item))){ if(setBodyItem(item)){ qSeq = bodyMotionItem->jointPosSeq(); positions = bodyMotionItem->linkPosSeq(); if(bodyMotionItem->hasRelativeZmpSeqItem()){ relativeZmpSeq = bodyMotionItem->relativeZmpSeq(); } } } else if((multiValueSeqItem = dynamic_cast(item))){ if(setBodyItem(item)){ qSeq = multiValueSeqItem->seq(); } } else if((multiAffine3SeqItem = dynamic_cast(item))){ if(setBodyItem(item)){ positions = multiAffine3SeqItem->seq(); } } if(bodyItem){ calcForwardKinematics = !(positions && positions->numParts() > 1); item->sigUpdated().connect(bind(&BodyMotionEngine::notifyUpdate, this)); } return (bodyItem); } bool setBodyItem(Item* item){ bodyItem = item->findOwnerItem(); if(bodyItem){ body = bodyItem->body(); } return (bodyItem); } virtual bool onTimeChanged(double time){ bool isActive = false; bool fkDone = false; if(qSeq){ isActive = setJointPositions(time); } if(positions){ isActive |= setLinkPositions(time); if(positions->numParts() == 1){ body->calcForwardKinematics(); // FK from the root fkDone = true; } } if(relativeZmpSeq){ if(calcForwardKinematics){ bodyItem->calcForwardKinematics(); fkDone = true; } isActive |= setRelativeZmp(time); } bodyItem->notifyKinematicStateChange(!fkDone && calcForwardKinematics); return isActive; } static inline bool clamp(int& frame, int numFrames) { if(frame < 0){ frame = 0; return false; } else if(frame >= numFrames){ frame = numFrames - 1; return false; } return true; } bool setLinkPositions(double time){ bool isValidTime = false; const int numLinks = positions->numParts(); const int numFrames = positions->numFrames(); if(numLinks > 0 && numFrames > 0){ int frame = positions->frameOfTime(time); isValidTime = clamp(frame, numFrames); for(int i=0; i < numLinks; ++i){ Link* link = body->link(i); const Affine3& position = positions->at(frame, i); link->p = position.translation(); link->R = position.linear(); } } return isValidTime; } bool setJointPositions(double time){ bool isValidTime = false; const int numJoints = qSeq->numParts(); int frame = qSeq->frameOfTime(time); const int numFrames = qSeq->numFrames(); if(numJoints > 0 && numFrames > 0){ isValidTime = clamp(frame, numFrames); MultiValueSeq::View q = qSeq->frame(frame); for(int i=0; i < numJoints; ++i){ Link* link = body->joint(i); link->q = q[i]; } } return isValidTime; } bool setRelativeZmp(double time){ bool isValidTime = false; int frame = relativeZmpSeq->frameOfTime(time); const int numFrames = relativeZmpSeq->numFrames(); if(numFrames > 0){ isValidTime = clamp(frame, numFrames); Link* rootLink = body->rootLink(); const Vector3& relativeZmp = (*relativeZmpSeq)[frame]; const Vector3 absZmp = rootLink->R * relativeZmp + rootLink->p; bodyItem->setZmp(absZmp); } return isValidTime; } }; typedef boost::intrusive_ptr BodyMotionEnginePtr; TimeSyncItemEnginePtr createBodyMotionEngine(Item* sourceItem) { BodyMotionEnginePtr engine = new BodyMotionEngine(); if(!engine->initialize(sourceItem)){ engine.reset(NULL); } return engine; } } void cnoid::initializeBodyMotionEngine(ExtensionManager& em) { em.timeSyncItemEngineManger().addEngineFactory(createBodyMotionEngine); } choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyMotionEngine.h000066400000000000000000000004271207742442300230160ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_BODY_MOTION_ENGINE_H_INCLUDED #define CNOID_BODYPLUGIN_BODY_MOTION_ENGINE_H_INCLUDED #include namespace cnoid { void initializeBodyMotionEngine(ExtensionManager& em); } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyMotionItem.cpp000066400000000000000000000204151207742442300230410ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "BodyMotionItem.h" #include "BodyItem.h" #include "KinematicFaultChecker.h" #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { bool confirm(const std::string& message) { return (QMessageBox::warning( 0, _("Warning"), message.c_str(), QMessageBox::Ok | QMessageBox::Cancel, QMessageBox::Ok) == QMessageBox::Ok); } bool fileIoSub(BodyMotionItem* item, std::ostream& os, bool loaded, bool isLoading) { if(!loaded){ os << item->motion()->ioErrorMessage(); } if(isLoading){ item->updateChildItemLineup(); } return loaded; } bool loadStandardYamlFormat(BodyMotionItem* item, const std::string& filename, std::ostream& os) { return fileIoSub(item, os, item->motion()->loadStandardYamlFormat(filename), true); } bool saveAsStandardYamlFormat(BodyMotionItem* item, const std::string& filename, std::ostream& os) { return fileIoSub(item, os, item->motion()->saveAsStandardYamlFormat(filename), false); } bool importHrpsysSeqFileSet(BodyMotionItem* item, const std::string& filename, std::ostream& os) { if(loadHrpsysSeqFileSet(*item->motion(), filename, os)){ item->updateChildItemLineup(); return true; } return false; } bool exportHrpsysSeqFileSet(BodyMotionItem* item, const std::string& filename, std::ostream& os) { double frameRate = item->motion()->frameRate(); if(frameRate != 200.0){ static format m1(_("The frame rate of a body motion exported as Hrpsys files should be standard value 200, " "but the frame rate of \"%1%\" is %2%. The exported data may cause a problem.\n\n" "Do you continue to export ?")); if(!confirm(str(m1 % item->name() % frameRate))){ return false; } } BodyPtr body; BodyItem* bodyItem = item->findOwnerItem(); if(bodyItem){ body = bodyItem->body(); KinematicFaultChecker* checker = KinematicFaultChecker::instance(); int numFaults = checker->checkFaults(bodyItem, item, os); if(numFaults > 0){ static string m2(_("A fault has been detected. Please check the report in the MessageView.\n\n" "Do you continue to export ?")); static format m3(_("%1% faults have been detected. Please check the report in the MessageView.\n\n" "Do you continue to export ?")); bool result; if(numFaults == 1){ result = confirm(m2); } else { result = confirm(str(m3 % numFaults)); } if(!result){ return false; } } } if(!item->hasRelativeZmpSeqItem()){ if(!confirm(_("There is no ZMP data. Do you continue to export ?"))){ return false; } } return saveHrpsysSeqFileSet(*item->motion(), body, filename, os); } bool bodyMotionItemPreFilter(BodyMotionItem* protoItem, Item* parentItem) { BodyItemPtr bodyItem = dynamic_cast(parentItem); if(!bodyItem){ bodyItem = parentItem->findOwnerItem(); } if(bodyItem){ int prevNumJoints = protoItem->jointPosSeq()->numParts(); int numJoints = bodyItem->body()->numJoints(); if(numJoints != prevNumJoints){ protoItem->jointPosSeq()->setNumParts(numJoints, true); } } return true; } bool bodyMotionItemPostFilter(BodyMotionItem* protoItem, Item* parentItem) { BodyItemPtr bodyItem = dynamic_cast(parentItem); if(!bodyItem){ bodyItem = parentItem->findOwnerItem(); } if(bodyItem){ BodyPtr body = bodyItem->body(); MultiValueSeqPtr qseq = protoItem->jointPosSeq(); int n = std::min(body->numJoints(), qseq->numParts()); for(int i=0; i < n; ++i){ Link* joint = body->joint(i); if(joint->defaultJointValue != 0.0){ MultiValueSeq::View view = qseq->part(i); std::fill(view.begin(), view.end(), joint->defaultJointValue); } } } return true; } } void cnoid::initializeBodyMotionItem(ExtensionManager& ext) { ItemManager& im = ext.itemManager(); im.registerClass(N_("BodyMotionItem")); im.addCreationPanel(new MultiSeqItemCreationPanel(_("Number of joints"))); im.addCreationPanelPreFilter(bodyMotionItemPreFilter); im.addCreationPanelPostFilter(bodyMotionItemPostFilter); im.addLoaderAndSaver( _("Body Motion"), "BODY-MOTION-YAML", "yaml", bind(loadStandardYamlFormat, _1, _2, _3), bind(saveAsStandardYamlFormat, _1, _2, _3)); im.addLoaderAndSaver( _("Hrpsys sequence file set"), "HRPSYS-SEQ-FILE-SET", "pos;vel;acc;hip;waist;gsens;zmp", bind(importHrpsysSeqFileSet, _1, _2, _3), bind(exportHrpsysSeqFileSet, _1, _2, _3), ItemManager::PRIORITY_CONVERSION); } BodyMotionItem::BodyMotionItem() : bodyMotion_(new BodyMotion()) { initialize(); } BodyMotionItem::BodyMotionItem(const BodyMotionItem& org) : MultiSeqItemBase(org), bodyMotion_(new BodyMotion(*org.bodyMotion_)) { initialize(); } void BodyMotionItem::initialize() { jointPosSeqItem_ = new MultiValueSeqItem(bodyMotion_->jointPosSeq()); jointPosSeqItem_->setName("q"); addSubItem(jointPosSeqItem_); jointPosSeqItem_->sigUpdated().connect( bind(&BodyMotionItem::onSubItemUpdated, this, jointPosSeqItem_.get())); linkPosSeqItem_ = new MultiAffine3SeqItem(bodyMotion_->linkPosSeq()); linkPosSeqItem_->setName("p,R"); addSubItem(linkPosSeqItem_); linkPosSeqItem_->sigUpdated().connect( bind(&BodyMotionItem::onSubItemUpdated, this, linkPosSeqItem_.get())); if(bodyMotion_->hasRelativeZmpSeq()){ relativeZmpSeqItem(); // create item } unsetAttribute(Item::TREE_EXPANDED_BY_DEFAULT); } Vector3SeqItem* BodyMotionItem::relativeZmpSeqItem() { if(!relativeZmpSeqItem_){ relativeZmpSeqItem_ = new Vector3SeqItem(bodyMotion_->relativeZmpSeq()); relativeZmpSeqItem_->setName("ZMP"); addSubItem(relativeZmpSeqItem_); relativeZmpSeqItem_->sigUpdated().connect( bind(&BodyMotionItem::onSubItemUpdated, this, relativeZmpSeqItem_.get())); } return relativeZmpSeqItem_.get(); } void BodyMotionItem::updateChildItemLineup() { if(!relativeZmpSeqItem_ && bodyMotion_->hasRelativeZmpSeq()){ relativeZmpSeqItem(); // create } } void BodyMotionItem::onSubItemUpdated(Item* childItem) { Item::notifyUpdate(); } void BodyMotionItem::notifyUpdate() { setInconsistencyWithLastAccessedFile(); jointPosSeqItem_->notifyUpdate(); linkPosSeqItem_->notifyUpdate(); if(relativeZmpSeqItem_){ relativeZmpSeqItem_->notifyUpdate(); } Item::notifyUpdate(); } ItemPtr BodyMotionItem::doDuplicate() const { return BodyMotionItemPtr(new BodyMotionItem(*this)); } bool BodyMotionItem::store(Archive& archive) { if(overwrite()){ archive.writeRelocatablePath("filename", lastAccessedFileName()); archive.write("format", lastAccessedFileFormatId()); return true; } return false; } bool BodyMotionItem::restore(const Archive& archive) { std::string filename, formatId; if(archive.readRelocatablePath("filename", filename) && archive.read("format", formatId)){ if(load(filename, formatId)){ return true; } } return false; } choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyMotionItem.h000066400000000000000000000037541207742442300225150ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_BODY_MOTION_ITEM_H_INCLUDED #define CNOID_BODYPLUGIN_BODY_MOTION_ITEM_H_INCLUDED #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT BodyMotionItem : public MultiSeqItemBase { public: BodyMotionItem(); virtual MultiSeqBasePtr seqBase() { return bodyMotion_; } inline const BodyMotionPtr& motion() { return bodyMotion_; } inline MultiValueSeqItem* jointPosSeqItem() { return jointPosSeqItem_.get(); } inline const MultiValueSeqPtr& jointPosSeq() { return bodyMotion_->jointPosSeq(); } inline MultiAffine3SeqItem* linkPosSeqItem() { return linkPosSeqItem_.get(); } inline const MultiAffine3SeqPtr& linkPosSeq() { return bodyMotion_->linkPosSeq(); } bool hasRelativeZmpSeqItem() { return bodyMotion_->hasRelativeZmpSeq(); } Vector3SeqItem* relativeZmpSeqItem(); inline Vector3SeqPtr relativeZmpSeq() { return relativeZmpSeqItem()->seq(); } virtual void notifyUpdate(); void updateChildItemLineup(); protected: BodyMotionItem(const BodyMotionItem& org); virtual ItemPtr doDuplicate() const; virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); private: BodyMotionPtr bodyMotion_; MultiValueSeqItemPtr jointPosSeqItem_; MultiAffine3SeqItemPtr linkPosSeqItem_; Vector3SeqItemPtr relativeZmpSeqItem_; void initialize(); void onSubItemUpdated(Item* childItem); }; typedef boost::intrusive_ptr BodyMotionItemPtr; void initializeBodyMotionItem(ExtensionManager& ext); } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyPlugin.cpp000066400000000000000000000053561207742442300222220ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include #include #include #include #include #include #include "BodyItem.h" #include "BodyMotionItem.h" #include "WorldItem.h" //#include "FilterDialogs.h" #include "KinematicFaultChecker.h" #include "BodyBar.h" #include "BodyLinkView.h" #include "LinkSelectionView.h" #include "JointSliderView.h" //#include "WorldView.h" #include "MultiValueSeqGraphView.h" #include "MultiAffine3SeqGraphView.h" #include "KinematicsBar.h" #include "SimulationBar.h" #include "KinematicsSimulatorItem.h" #include "DynamicsSimulatorItem.h" #include "BodyMotionEngine.h" #include "SceneBodyManager.h" #include "SceneWorld.h" #include "gettext.h" using namespace cnoid; namespace { class BodyPlugin : public Plugin { public: BodyPlugin() : Plugin("Body") { } virtual bool initialize(){ Body::addCustomizerDirectory( App::topDirectory() + "/" + CNOID_PLUGIN_SUBDIR + "/customizer"); initializeBodyItem(*this); initializeBodyMotionItem(*this); initializeWorldItem(*this); initializeKinematicsSimulatorItem(*this); initializeDynamicsSimulatorItem(*this); initializeBodyMotionEngine(*this); //initializeFilterDialogs(*this); KinematicFaultChecker::initialize(*this); addToolBar(BodyBar::instance()); addToolBar(KinematicsBar::instance()); addToolBar(SimulationBar::initialize(this)); addView(new LinkSelectionView()); addView(new BodyLinkView()); addView(new JointSliderView()); //addView(new WorldView()); addView(new MultiValueSeqGraphView()); addView(new MultiAffine3SeqGraphView()); manage(new SceneBodyManager(*this)); manage(new SceneWorldManager()); return true; } virtual const char* description() { static std::string text = str(boost::format(_("Body Plugin Version %1%\n")) % CNOID_FULL_VERSION_STRING) + "\n" + _("This plugin has been developed by Shin'ichiro Nakaoka and Choreonoid Development Team, AIST, " "and is distributed as a part of the Choreonoid package.\n" "\n") + LGPLtext() + "\n" + _("The Collision deteciton module used in this plugin is implemented using " "the OPCODE library (http://www.codercorner.com/Opcode.htm).\n"); return text.c_str(); } }; } CNOID_IMPLEMENT_PLUGIN_ENTRY(BodyPlugin); choreonoid-1.1.0+dfsg/src/BodyPlugin/BodyPlugin.qrc000066400000000000000000000016331207742442300222170ustar00rootroot00000000000000 icons/fkik.png icons/fk.png icons/ik.png icons/rotation.png icons/block.png icons/collisionoutline.png icons/storepose.png icons/restorepose.png icons/origin.png icons/initialpose.png icons/stdpose.png icons/right-to-left.png icons/left-to-right.png icons/flip.png icons/center-cm.png icons/zmp-to-cm.png icons/cm-to-zmp.png icons/center-zmp.png icons/left-zmp.png icons/right-zmp.png icons/stancelength.png icons/startsimulation.png icons/stopsimulation.png choreonoid-1.1.0+dfsg/src/BodyPlugin/CMakeLists.txt000066400000000000000000000023231207742442300221710ustar00rootroot00000000000000 # @author Shin'ichiro Nakaoka # set(CMAKE_BUILD_TYPE Debug) set(sources BodyPlugin.cpp BodyItem.cpp WorldItem.cpp BodyMotionItem.cpp SimulatorItem.cpp KinematicsSimulatorItem.cpp DynamicsSimulatorItem.cpp BodyMotionEngine.cpp KinematicFaultChecker.cpp #FilterDialogs.cpp SceneBodyManager.cpp SceneBody.cpp SceneWorld.cpp OsgCollision.cpp BodyBar.cpp KinematicsBar.cpp SimulationBar.cpp LinkTreeWidget.cpp LinkSelectionView.cpp BodyLinkView.cpp JointSliderView.cpp #WorldView.cpp MultiAffine3SeqGraphView.cpp MultiValueSeqGraphView.cpp ) set(headers BodyItem.h WorldItem.h BodyMotionItem.h SimulatorItem.h KinematicFaultChecker.h SceneBodyManager.h SceneBody.h SceneWorld.h BodyBar.h KinematicsBar.h LinkTreeWidget.h LinkSelectionView.h exportdecl.h gettext.h ) QT4_WRAP_CPP(sources LinkTreeWidget.h ) QT4_ADD_RESOURCES(RC_SRCS BodyPlugin.qrc) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) set(target CnoidBodyPlugin) make_gettext_mofiles(mofiles) add_library(${target} SHARED ${sources} ${headers} ${mofiles} ${RC_SRCS}) target_link_libraries(${target} CnoidUtil CnoidBase CnoidBody) apply_common_setting_for_plugin(${target} "${headers}") choreonoid-1.1.0+dfsg/src/BodyPlugin/DynamicsSimulatorItem.cpp000066400000000000000000000321611207742442300244260ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "DynamicsSimulatorItem.h" #include "WorldItem.h" #include "BodyItem.h" #include "BodyMotionItem.h" #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace cnoid; // for Windows #undef min #undef max namespace { const bool TRACE_FUNCTIONS = false; class BodyUnit { public: int frame; double frameRate; BodyPtr body; MultiValueSeqPtr qseqRef; MultiValueSeqPtr qseqResultBuf; MultiValueSeqPtr qseqResult; MultiAffine3SeqItemPtr rootResultItem; MultiAffine3SeqPtr rootResultBuf; MultiAffine3SeqPtr rootResult; bool initialize(BodyItemPtr bodyItem, double worldFrameRate); bool setReferenceState(int newFrame, double frameRate, bool& putResult); void putResultToBuf(); bool flushBuf(); }; } namespace cnoid { class DSIImpl { public: DynamicsSimulatorItem* self; World world; double worldFrameRate; double staticFriction; double slipFriction; double culling_thresh; double epsilon; vector bodyUnits; vector bodyUnitsToPutResult; vector bodyUnitsToNotifyUpdate; int currentFrame; int frameAtLastBufferWriting; DSIImpl(DynamicsSimulatorItem* self); DSIImpl(DynamicsSimulatorItem* self, const DSIImpl& org); void initialize(); bool doStartSimulation(); bool setupBodies(); void addBody(BodyItemPtr bodyItem); bool doStepSimulation(); double doFlushResults(); void doStopSimulation(); bool onFrictionPropertyChanged(double mu, int type); }; } void cnoid::initializeDynamicsSimulatorItem(ExtensionManager& ext) { ext.itemManager().registerClass(N_("DynamicsSimulatorItem")); ext.itemManager().addCreationPanel(); } bool BodyUnit::initialize(BodyItemPtr bodyItem, double worldFrameRate) { if(TRACE_FUNCTIONS){ cout << "BodyUnit::initialize()" << endl; } frame = 0; body = bodyItem->body()->duplicate(); if(body->isStaticModel()){ return true; } const int numJoints = body->numJoints(); ItemList motionItems = bodyItem->getSubItems(); BodyMotionItemPtr motionItem; if(motionItems.empty()){ motionItem = new BodyMotionItem(); motionItem->setName("sim"); motionItem->motion()->setFrameRate(worldFrameRate); qseqResult = motionItem->jointPosSeq(); qseqResult->setNumParts(numJoints); motionItem->motion()->setNumFrames(1); for(int i=0; i < numJoints; ++i){ qseqResult->at(0, i) = body->joint(i)->q; } bodyItem->addChildItem(motionItem); qseqResultBuf.reset(new MultiValueSeq(numJoints, 0, worldFrameRate)); } else { // prefer the first checked item for(size_t i=0; i < motionItems.size(); ++i){ if(ItemTreeView::mainInstance()->isItemChecked(motionItems[i])){ motionItem = motionItems[i]; break; } } if(!motionItem){ motionItem = motionItems[0]; } qseqRef = motionItem->jointPosSeq(); } frameRate = motionItem->motion()->frameRate(); rootResultItem = motionItem->linkPosSeqItem(); rootResult = motionItem->linkPosSeq(); rootResultBuf.reset(new MultiAffine3Seq(1, 0, frameRate)); rootResult->setFrameRate(frameRate); rootResult->setNumParts(1); rootResult->setNumFrames(1); Link* rootLink = body->rootLink(); Affine3& initialPos = rootResult->at(0, 0); initialPos.translation() = rootLink->p; initialPos.linear() = rootLink->R; rootLink->v.setZero(); rootLink->dv.setZero(); rootLink->w.setZero(); rootLink->dw.setZero(); rootLink->vo.setZero(); rootLink->dvo.setZero(); // set the high-gain mode for(int i=0; i < body->numJoints(); ++i){ Link* joint = body->joint(i); joint->isHighGainMode = true; joint->u = 0.0; joint->dq = 0.0; if(!qseqRef || qseqRef->numFrames() < 2){ joint->ddq = 0.0; } else { //double qNext = qseqRef->atSeqFrame(i, 1); //double dqNext = (qNext - joint->q) * frameRate; //joint->ddq = dqNext * frameRate; joint->ddq = 0.0; } } body->clearExternalForces(); body->calcForwardKinematics(true, true); return true; } bool BodyUnit::setReferenceState(int newFrame, double worldFrameRate, bool& putResult) { putResult = false; if(TRACE_FUNCTIONS){ cout << "BodyUnit::updateState()" << endl; } if(!qseqRef){ putResult = true; frame = newFrame; return true; } if(newFrame >= qseqRef->numFrames()){ return false; } if(newFrame <= frame){ return true; } frame = newFrame; MultiValueSeq::View qRef = qseqRef->frame(frame); const int n = std::min(body->numJoints(), qseqRef->numParts()); for(int i=0; i < n; ++i){ Link* joint = body->joint(i); double qNext = qRef[i]; double dqNext = (qNext - joint->q) * worldFrameRate; joint->ddq = (dqNext - joint->dq) * worldFrameRate; joint->dq = dqNext; joint->q = qNext; } putResult = true; return true; } void BodyUnit::putResultToBuf() { const int bufFrame = rootResultBuf->numFrames(); if(qseqResultBuf){ qseqResultBuf->setNumFrames(bufFrame + 1); MultiValueSeq::View qResult = qseqResultBuf->frame(bufFrame); const int n = body->numJoints(); for(int i=0; i < n; ++i){ qResult[i] = body->joint(i)->q; } } rootResultBuf->setNumFrames(bufFrame + 1); Link* rootLink = body->rootLink(); Affine3& pos = rootResultBuf->at(bufFrame, 0); pos.translation() = rootLink->p; pos.linear() = rootLink->R; } bool BodyUnit::flushBuf() { const int frame = rootResult->numFrames(); const int numBufFrames = rootResultBuf->numFrames(); if(numBufFrames > 0){ if(qseqResult){ const int n = body->numJoints(); qseqResult->setNumFrames(frame + numBufFrames); for(int i=0; i < numBufFrames; ++i){ MultiValueSeq::View qResult = qseqResult->frame(frame + i); MultiValueSeq::View qResultBuf = qseqResultBuf->frame(i); for(int j=0; j < n; ++j){ qResult[j] = qResultBuf[j]; } } qseqResultBuf->setNumFrames(0); } rootResult->setNumFrames(frame + numBufFrames); for(int i=0; i < numBufFrames; ++i){ rootResult->at(frame + i, 0) = rootResultBuf->at(i, 0); } rootResultBuf->setNumFrames(0); } return (numBufFrames > 0); } DynamicsSimulatorItem::DynamicsSimulatorItem() { impl = new DSIImpl(this); } DSIImpl::DSIImpl(DynamicsSimulatorItem* self) : self(self) { initialize(); } DynamicsSimulatorItem::DynamicsSimulatorItem(const DynamicsSimulatorItem& org) : SimulatorItem(org), impl(new DSIImpl(this, *org.impl)) { } DSIImpl::DSIImpl(DynamicsSimulatorItem* self, const DSIImpl& org) : self(self) { initialize(); staticFriction = org.staticFriction; slipFriction = org.slipFriction; } void DSIImpl::initialize() { world.setRungeKuttaMethod(); world.setGravityAcceleration(Vector3(0.0, 0.0, 9.8)); world.enableSensors(true); staticFriction = 1.0; slipFriction = 1.0; culling_thresh = 0.01; epsilon = 0.0; } DynamicsSimulatorItem::~DynamicsSimulatorItem() { delete impl; } ItemPtr DynamicsSimulatorItem::doDuplicate() const { return new DynamicsSimulatorItem(*this); } QWidget* DynamicsSimulatorItem::settingPanel() { return 0; } bool DynamicsSimulatorItem::doStartSimulation() { return impl->doStartSimulation(); } bool DSIImpl::doStartSimulation() { worldFrameRate = TimeBar::instance()->frameRate(); world.clearBodies(); world.constraintForceSolver.clearCollisionCheckLinkPairs(); bool result = setupBodies(); if(result){ currentFrame = 0; frameAtLastBufferWriting = 0; world.setTimeStep(1.0 / worldFrameRate); world.setCurrentTime(0.0); world.initialize(); } return result; } bool DSIImpl::setupBodies() { if(TRACE_FUNCTIONS){ cout << "DSIImpl::setupBodies()" << endl; } bodyUnits.clear(); WorldItemPtr worldItem = self->findOwnerItem(); if(worldItem){ ItemList bodyItems = worldItem->getBodyItems(); for(size_t i=0; i < bodyItems.size(); ++i){ BodyUnit unit; if(unit.initialize(bodyItems[i], worldFrameRate)){ bodyUnits.push_back(unit); world.addBody(bodyUnits[i].body); } } } // set all the combinations of collision check link pairs for(size_t b1 = 0; b1 < bodyUnits.size(); ++b1){ BodyPtr& body1 = bodyUnits[b1].body; for(size_t b2 = b1 + 1; b2 < bodyUnits.size(); ++b2){ BodyPtr& body2 = bodyUnits[b2].body; for(int l1 = 0; l1 < body1->numLinks(); ++l1){ Link* link1 = body1->link(l1); for(int l2 = 0; l2 < body2->numLinks(); ++l2){ Link* link2 = body2->link(l2); if(link1 != link2){ world.constraintForceSolver.addCollisionCheckLinkPair (b1, link1, b2, link2, staticFriction, slipFriction, culling_thresh, epsilon); } } } } } // remove static-model bodies vector::iterator p = bodyUnits.begin(); while(p != bodyUnits.end()){ if(p->body->isStaticModel()){ p = bodyUnits.erase(p); } else { ++p; } } return !bodyUnits.empty(); } bool DynamicsSimulatorItem::doStepSimulation() { return impl->doStepSimulation(); } bool DSIImpl::doStepSimulation() { currentFrame++; world.constraintForceSolver.clearExternalForces(); bool doContinue = false; bodyUnitsToPutResult.clear(); for(size_t i=0; i < bodyUnits.size(); ++i){ BodyUnit& unit = bodyUnits[i]; int frame = static_cast(currentFrame * unit.frameRate / worldFrameRate); bool putResult; if(unit.setReferenceState(frame, worldFrameRate, putResult)){ if(putResult){ bodyUnitsToPutResult.push_back(&unit); } doContinue = true; } } if(doContinue){ static int counter = 0; world.calcNextState(); self->lockResults(); for(size_t i=0; i < bodyUnitsToPutResult.size(); ++i){ bodyUnitsToPutResult[i]->putResultToBuf(); } frameAtLastBufferWriting = currentFrame; if(++counter == 10){ self->requestToFlushResults(); counter = 0; } self->unlockResults(); } return doContinue; } double DynamicsSimulatorItem::doFlushResults() { return impl->doFlushResults(); } double DSIImpl::doFlushResults() { bodyUnitsToNotifyUpdate.clear(); self->lockResults(); for(size_t i=0; i < bodyUnits.size(); ++i){ if(bodyUnits[i].flushBuf()){ bodyUnitsToNotifyUpdate.push_back(&bodyUnits[i]); } } int frame = frameAtLastBufferWriting; self->unlockResults(); for(size_t i=0; i < bodyUnitsToNotifyUpdate.size(); ++i){ bodyUnitsToNotifyUpdate[i]->rootResultItem->notifyUpdate(); } return (frame / worldFrameRate); } double DynamicsSimulatorItem::doStopSimulation() { return (impl->currentFrame / impl->worldFrameRate); } bool DSIImpl::onFrictionPropertyChanged(double mu, int type) { if(mu > 0.0){ if(type == 0){ staticFriction = mu; } else if(type == 1){ slipFriction = mu; } return true; } return false; } void DynamicsSimulatorItem::doPutProperties(PutPropertyFunction& putProperty) { putProperty(_("Static friction"), impl->staticFriction, (bind(&DSIImpl::onFrictionPropertyChanged, impl, _1, 0))); putProperty(_("Slip friction"), impl->slipFriction, (bind(&DSIImpl::onFrictionPropertyChanged, impl, _1, 1))); } bool DynamicsSimulatorItem::store(Archive& archive) { archive.write("staticFriction", impl->staticFriction); archive.write("slipFriction", impl->slipFriction); return true; } bool DynamicsSimulatorItem::restore(const Archive& archive) { archive.read("staticFriction", impl->staticFriction); archive.read("slipFriction", impl->slipFriction); return true; } choreonoid-1.1.0+dfsg/src/BodyPlugin/DynamicsSimulatorItem.h000066400000000000000000000021751207742442300240750ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_DYNAMICS_SIMULATOR_ITEM_H_INCLUDED #define CNOID_BODYPLUGIN_DYNAMICS_SIMULATOR_ITEM_H_INCLUDED #include "SimulatorItem.h" namespace cnoid { class DSIImpl; class DynamicsSimulatorItem : public SimulatorItem { public: DynamicsSimulatorItem(); DynamicsSimulatorItem(const DynamicsSimulatorItem& org); virtual ~DynamicsSimulatorItem(); virtual bool doStartSimulation(); virtual bool doStepSimulation(); virtual double doFlushResults(); virtual double doStopSimulation(); protected: virtual QWidget* settingPanel(); virtual ItemPtr doDuplicate() const; virtual void doPutProperties(PutPropertyFunction& putProperty); virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); private: DSIImpl* impl; friend class DSIImpl; }; typedef boost::intrusive_ptr DynamicsSimulatorItemPtr; void initializeDynamicsSimulatorItem(ExtensionManager& ext); } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/FilterDialogs.cpp000066400000000000000000000262011207742442300226660ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "FilterDialogs.h" #include "BodyItem.h" #include "LinkSelectionView.h" #include #include #include #include #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { class VelocityLimitFilterDialog : public Gtk::Dialog { public: VelocityLimitFilterDialog(); protected: virtual void on_response(int id); private: Gtk::CheckButton onlySelectedCheck; Gtk::SpinButton percentSpin; Gtk::CheckButton pollardCheck; Gtk::SpinButton ksSpin; Gtk::CheckButton gaussianCheck; Gtk::SpinButton gaussianSigmaSpin; Gtk::SpinButton gaussianRangeSpin; }; VelocityLimitFilterDialog::VelocityLimitFilterDialog() : Gtk::Dialog(_("Velocity Limiting Filter"), MainWindow::instance()) { set_modal(false); Gtk::VBox* vbox = get_vbox(); Gtk::HBox* hbox = Gtk::manage(new Gtk::HBox()); onlySelectedCheck.set_label(_("Selected joints only")); onlySelectedCheck.set_active(false); hbox->pack_start(onlySelectedCheck, Gtk::PACK_SHRINK); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); hbox = Gtk::manage(new Gtk::HBox()); hbox->pack_start(*Gtk::manage(new Gtk::Label("Ratio")), Gtk::PACK_SHRINK); percentSpin.set_range(1.0, 100.0); percentSpin.set_increments(1.0, 10.0); percentSpin.set_value(100.0); hbox->pack_start(percentSpin, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label("%")), Gtk::PACK_SHRINK); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); hbox = Gtk::manage(new Gtk::HBox()); pollardCheck.set_label(_("Pollard method")); pollardCheck.set_active(false); hbox->pack_start(pollardCheck, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label(",")), Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label("ks")), Gtk::PACK_SHRINK); ksSpin.set_digits(1); ksSpin.set_range(0.0, 9.9); ksSpin.set_increments(0.1, 1.0); ksSpin.set_value(2.0); hbox->pack_start(ksSpin, Gtk::PACK_SHRINK); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); hbox = Gtk::manage(new Gtk::HBox()); gaussianCheck.set_label(_("Gaussian Filter")); gaussianCheck.set_active(true); hbox->pack_start(gaussianCheck, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label("sigma")), Gtk::PACK_SHRINK); gaussianSigmaSpin.set_digits(1); gaussianSigmaSpin.set_range(0.1, 9.9); gaussianSigmaSpin.set_increments(0.1, 1.0); gaussianSigmaSpin.set_value(3.0); hbox->pack_start(gaussianSigmaSpin, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label("range")), Gtk::PACK_SHRINK); gaussianRangeSpin.set_digits(0); gaussianRangeSpin.set_range(1, 99); gaussianRangeSpin.set_increments(1, 5); gaussianRangeSpin.set_value(7); hbox->pack_start(gaussianRangeSpin, Gtk::PACK_SHRINK); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); show_all_children(); add_button(_("Apply"), Gtk::RESPONSE_OK); add_button(_("Cancel"), Gtk::RESPONSE_CANCEL); } void VelocityLimitFilterDialog::on_response(int id) { if(id == Gtk::RESPONSE_OK){ ItemList items = ItemTreeView::mainInstance()->selectedItems(); for(size_t i=0; i < items.size(); ++i){ BodyItemPtr bodyItem = items[i]->findOwnerItem(); if(bodyItem){ bool result; std::ostream& os = MessageView::mainInstance()->cout(); // \todo The following code has not completely been implemented. if(onlySelectedCheck.get_active()){ dynamic_bitset<> linkSelection = LinkSelectionView::mainInstance()->getLinkSelection(bodyItem); BodyPtr body = bodyItem->body(); double r = percentSpin.get_value() / 100.0; int n = body->numJoints(); for(int j=0; j < n; ++j){ Link* joint = body->joint(j); if(linkSelection[joint->index]){ std::cout << "joint " << joint->name() << ", limit = " << joint->uvlimit << ", ratio = " << r << endl; result = applyVelocityLimitFilter2(*items[i]->seq(), j, joint->uvlimit * r); } } } else { if(pollardCheck.get_active()){ result = applyPollardVelocityLimitFilter( *items[i]->seq(), bodyItem->body(), ksSpin.get_value(), os); } else { result = applyVelocityLimitFilter( *items[i]->seq(), bodyItem->body(), os); } if(gaussianCheck.get_active()){ applyGaussianFilter( *items[i]->seq(), gaussianSigmaSpin.get_value(), gaussianRangeSpin.get_value_as_int(), os); result = true; } } if(result){ items[i]->notifyUpdate(); } } } } hide(); } class RangeLimitFilterDialog : public Gtk::Dialog { public: RangeLimitFilterDialog(); protected: virtual void on_response(int id); private: Gtk::SpinButton limitGradSpin; Gtk::SpinButton edgeGradRatioSpin; Gtk::SpinButton marginSpin; }; RangeLimitFilterDialog::RangeLimitFilterDialog() : Gtk::Dialog(_("Range Limiting Filter"), MainWindow::instance()) { set_modal(false); Gtk::VBox* vbox = get_vbox(); Gtk::HBox* hbox = Gtk::manage(new Gtk::HBox()); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("limit grad."))), Gtk::PACK_SHRINK); limitGradSpin.set_digits(1); limitGradSpin.set_range(0.1, 0.9); limitGradSpin.set_increments(0.1, 0.2); limitGradSpin.set_value(0.4); hbox->pack_start(limitGradSpin, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("edge grad. ratio"))), Gtk::PACK_SHRINK); edgeGradRatioSpin.set_digits(1); edgeGradRatioSpin.set_range(0.1, 1.0); edgeGradRatioSpin.set_increments(0.1, 0.2); edgeGradRatioSpin.set_value(0.5); hbox->pack_start(edgeGradRatioSpin, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("margin"))), Gtk::PACK_SHRINK); marginSpin.set_digits(3); marginSpin.set_range(0.0, 9.999); marginSpin.set_increments(0.001, 0.1); marginSpin.set_value(0.0); hbox->pack_start(marginSpin, Gtk::PACK_SHRINK); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); show_all_children(); add_button(_("Apply"), Gtk::RESPONSE_OK); add_button(_("Cancel"), Gtk::RESPONSE_CANCEL); } void RangeLimitFilterDialog::on_response(int id) { if(id == Gtk::RESPONSE_OK){ ItemList items = ItemTreeView::mainInstance()->selectedItems(); for(size_t i=0; i < items.size(); ++i){ BodyItemPtr bodyItem = items[i]->findOwnerItem(); if(bodyItem){ std::ostream& os = MessageView::mainInstance()->cout(); applyRangeLimitFilter(*items[i]->seq(), bodyItem->body(), limitGradSpin.get_value(), edgeGradRatioSpin.get_value(), marginSpin.get_value(), os); items[i]->notifyUpdate(); } } } hide(); } void mergeSelectedMultiValueSeqItems() { ItemList items = ItemTreeView::mainInstance()->selectedItems(); if(!items.empty()){ string name; ItemList<> commonPath; MultiValueSeqItemPtr mergedItem = new MultiValueSeqItem(); MultiValueSeqPtr merged = mergedItem->seq(); for(size_t i=0; i < items.size(); ++i){ MultiValueSeqItemPtr item = items[i]; MultiValueSeqPtr seq = item->seq(); merged->setFrameRate(seq->frameRate()); for(int j=0; j < seq->numParts(); ++j){ //if(seq->isSeqValid(j)){ if(j >= merged->numParts() || seq->numFrames() > merged->numFrames()){ merged->setDimension(seq->numFrames(), j+1); } MultiValueSeq::View src = seq->part(j); std::copy(src.begin(), src.end(), merged->part(j).begin()); //} } ItemList<> path; for(Item* parent = item->parent(); parent; parent = parent->parent()){ path.push_front(parent); } if(i==0){ name = item->name(); commonPath = path; } else { name += " + " + item->name(); ItemList<> prev(commonPath); commonPath.clear(); ItemList<>::iterator p = prev.begin(); ItemList<>::iterator q = path.begin(); while(p != prev.end() && q != path.end()){ if(*p == *q){ commonPath.push_back(*p); } ++p; ++q; } } } mergedItem->setName(name); ItemPtr parentItem = commonPath.back(); parentItem->addChildItem(mergedItem); } } } void cnoid::initializeFilterDialogs(ExtensionManager& ext) { Gtk::Dialog* dialog; MenuManager& mm = ext.menuManager(); mm.setPath("/Filters"); dialog = ext.manage(new VelocityLimitFilterDialog()); mm.addItem(_("Velocity Limiting Filter")) ->signal_activate().connect(sigc::mem_fun(*dialog, &Gtk::Widget::show)); dialog = ext.manage(new RangeLimitFilterDialog()); mm.addItem(_("Range Limiting Filter")) ->signal_activate().connect(sigc::mem_fun(*dialog, &Gtk::Widget::show)); mm.addItem(_("Merge selected MultiValueSeqItems (Temporary version)")) ->signal_activate().connect(sigc::ptr_fun(&mergeSelectedMultiValueSeqItems)); } choreonoid-1.1.0+dfsg/src/BodyPlugin/FilterDialogs.h000066400000000000000000000004121207742442300223270ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_BODYPLUGIN_FILTER_DIALOGS_H_INCLUDED #define CNOID_BODYPLUGIN_FILTER_DIALOGS_H_INCLUDED namespace cnoid { class ExtensionManager; void initializeFilterDialogs(ExtensionManager& ext); } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/JointSliderView.cpp000066400000000000000000000442031207742442300232210ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "JointSliderView.h" #include "BodyItem.h" #include "BodyBar.h" #include "LinkSelectionView.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class JointSliderViewImpl; // slider resolution static const double r = 1000000.0; } namespace { class SliderUnit { public: SliderUnit(JointSliderViewImpl* view, int index); void setName(const QString& name){ nameLabel.setText(name); nameLabel.show(); } void hideName(){ nameLabel.hide(); } void setId(int id){ idLabel.setText(QString("%1:").arg(id)); idLabel.show(); } void hideId(){ idLabel.hide(); } void setRange(double lower, double upper){ double l = std::max(degree(lower), -360.0); double u = std::min(degree(upper), 360.0); slider.blockSignals(true); slider.setRange(l * r, u * r); lowerLimitLabel.setText(QString::number(l, 'f', 1)); upperLimitLabel.setText(QString::number(u, 'f', 1)); slider.blockSignals(false); } double value() { return radian(spin.value()); } void setValue(double value) { slider.blockSignals(true); spin.blockSignals(true); double degvalue = degree(value); spin.setValue(degvalue); slider.setValue(degvalue * r); spin.blockSignals(false); slider.blockSignals(false); } void onSliderValueChanged(double value){ spin.blockSignals(true); spin.setValue(value / r); spin.blockSignals(false); slotOnValueChanged(); } void onSpinValueChanged(double value){ slider.blockSignals(true); slider.setValue(value * r); slider.blockSignals(false); slotOnValueChanged(); } void enableSpinEntry(bool isEnabled){ if(isEnabled){ spin.show(); } else { spin.hide(); } } void enableSlider(bool isEnabled){ if(isEnabled){ lowerLimitLabel.show(); slider.show(); upperLimitLabel.show(); } else { lowerLimitLabel.hide(); slider.hide(); upperLimitLabel.hide(); } } void setSlotOnValueChanged(const function& slot){ slotOnValueChanged = slot; } void removeWidgesFrom(QGridLayout& grid){ grid.removeWidget(&idLabel); grid.removeWidget(&nameLabel); grid.removeWidget(&spin); grid.removeWidget(&lowerLimitLabel); grid.removeWidget(&slider); grid.removeWidget(&upperLimitLabel); } QLabel idLabel; QLabel nameLabel; DoubleSpinBox spin; QLabel lowerLimitLabel; Slider slider; QLabel upperLimitLabel; function slotOnValueChanged; }; } namespace cnoid { class JointSliderViewImpl : public boost::signals::trackable, public QObject { public: JointSliderViewImpl(JointSliderView* self); ~JointSliderViewImpl(); JointSliderView* self; ToggleToolButton showAllToggle; ToggleToolButton jointIdToggle; ToggleToolButton nameToggle; ToggleToolButton labelOnLeftToggle; SpinBox numColumnsSpin; ToggleToolButton putSpinEntryCheck; ToggleToolButton putSliderCheck; QScrollArea scrollArea; QWidget sliderGridBase; QGridLayout sliderGrid; vector activeJointIds; vector jointSliders; BodyItemPtr currentBodyItem; signals::connection connectionOfKinematicStateChanged; signals::connection connectionOfBodyItemDetachedFromRoot; signals::connection connectionOfLinkSelectionChanged; void updateSliderGrid(); void attachSliderUnits(SliderUnit* unit, int row, int col); void initializeSliders(int num); void onNumColumnsChanged(int n); bool eventFilter(QObject* object, QEvent* event); bool onSliderKeyPressEvent(Slider* slider, QKeyEvent* event); void focusSlider(int index); void onJointSliderChanged(int sliderIndex); void onKinematicStateChanged(); void onCurrentBodyItemChanged(BodyItem* bodyItem); void enableConnectionToSigKinematicStateChanged(bool on); bool storeState(Archive& archive); bool restoreState(const Archive& archive); }; } SliderUnit::SliderUnit(JointSliderViewImpl* view, int index) : idLabel(&view->sliderGridBase), nameLabel(&view->sliderGridBase), spin(&view->sliderGridBase), lowerLimitLabel(&view->sliderGridBase), slider(Qt::Horizontal, &view->sliderGridBase), upperLimitLabel(&view->sliderGridBase) { idLabel.setAlignment(Qt::AlignRight | Qt::AlignVCenter); nameLabel.setAlignment(Qt::AlignCenter); lowerLimitLabel.setAlignment(Qt::AlignCenter); upperLimitLabel.setAlignment(Qt::AlignCenter); spin.setAlignment(Qt::AlignCenter); spin.setDecimals(1); spin.setRange(-999.9, 999.9); spin.setSingleStep(0.1); spin.sigValueChanged().connect(bind(&SliderUnit::onSpinValueChanged, this, _1)); slider.setSingleStep(0.1 * r); slider.setProperty("JointSliderIndex", index); slider.installEventFilter(view); slider.sigValueChanged().connect(bind(&SliderUnit::onSliderValueChanged, this, _1)); } JointSliderView::JointSliderView() { impl = new JointSliderViewImpl(this); } JointSliderViewImpl::JointSliderViewImpl(JointSliderView* self) : self(self) { self->setName(N_("Joint Sliders")); self->setDefaultLayoutArea(View::CENTER); QVBoxLayout* vbox = new QVBoxLayout(); vbox->setSpacing(0); QHBoxLayout* hbox = new QHBoxLayout(); hbox->setSpacing(0); showAllToggle.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); showAllToggle.setText(_("All")); showAllToggle.setToolTip(_("Show all the joints including unselected ones")); showAllToggle.setChecked(false); showAllToggle.sigToggled().connect(bind(&JointSliderViewImpl::updateSliderGrid, this)); hbox->addWidget(&showAllToggle); jointIdToggle.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); jointIdToggle.setText(_("ID")); jointIdToggle.setToolTip(_("Show joint IDs")); jointIdToggle.setChecked(false); jointIdToggle.sigToggled().connect(bind(&JointSliderViewImpl::updateSliderGrid, this)); hbox->addWidget(&jointIdToggle); nameToggle.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); nameToggle.setText(_("Name")); nameToggle.setToolTip(_("Show joint names")); nameToggle.setChecked(true); nameToggle.sigToggled().connect(bind(&JointSliderViewImpl::updateSliderGrid, this)); hbox->addWidget(&nameToggle); putSpinEntryCheck.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); putSpinEntryCheck.setText(_("Entry")); putSpinEntryCheck.setToolTip(_("Show spin entries for numerical input")); putSpinEntryCheck.setChecked(true); putSpinEntryCheck.sigToggled().connect(bind(&JointSliderViewImpl::updateSliderGrid, this)); hbox->addWidget(&putSpinEntryCheck); putSliderCheck.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); putSliderCheck.setText(_("Slider")); putSliderCheck.setToolTip(_("Show sliders for chaning joint positions")); putSliderCheck.setChecked(true); putSliderCheck.sigToggled().connect(bind(&JointSliderViewImpl::updateSliderGrid, this)); hbox->addWidget(&putSliderCheck); labelOnLeftToggle.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); labelOnLeftToggle.setText(_("IL")); labelOnLeftToggle.setToolTip(_("Put all the components for each joint in-line")); labelOnLeftToggle.setChecked(true); labelOnLeftToggle.sigToggled().connect(bind(&JointSliderViewImpl::updateSliderGrid, this)); hbox->addWidget(&labelOnLeftToggle); hbox->addSpacing(4); hbox->addWidget(new VSeparator()); hbox->addSpacing(4); numColumnsSpin.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); numColumnsSpin.setToolTip(_("The number of columns")); numColumnsSpin.setRange(1, 9); numColumnsSpin.setValue(1); numColumnsSpin.sigValueChanged().connect( bind(&JointSliderViewImpl::onNumColumnsChanged, this, _1)); hbox->addWidget(&numColumnsSpin); hbox->addStretch(); vbox->addLayout(hbox); sliderGrid.setSpacing(0); QVBoxLayout* gridVBox = new QVBoxLayout(); gridVBox->addLayout(&sliderGrid); gridVBox->addStretch(); sliderGridBase.setLayout(gridVBox); scrollArea.setFrameShape(QFrame::NoFrame); scrollArea.setWidgetResizable(true); scrollArea.setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); scrollArea.setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); scrollArea.setWidget(&sliderGridBase); vbox->addWidget(&scrollArea, 1); self->setLayout(vbox); updateSliderGrid(); BodyBar::instance()->sigCurrentBodyItemChanged().connect( bind(&JointSliderViewImpl::onCurrentBodyItemChanged, this, _1)); self->sigActivated().connect(bind(&JointSliderViewImpl::enableConnectionToSigKinematicStateChanged, this, true)); self->sigDeactivated().connect(bind(&JointSliderViewImpl::enableConnectionToSigKinematicStateChanged, this, false)); } JointSliderView::~JointSliderView() { delete impl; } JointSliderViewImpl::~JointSliderViewImpl() { for(size_t i=0; i < jointSliders.size(); ++i){ delete jointSliders[i]; } } void JointSliderViewImpl::updateSliderGrid() { if(!currentBodyItem){ initializeSliders(0); } else { BodyPtr body = currentBodyItem->body(); int numJoints = body->numJoints(); if(!showAllToggle.isChecked()){ const dynamic_bitset<>& linkSelection = LinkSelectionView::mainInstance()->getLinkSelection(currentBodyItem); activeJointIds.clear(); for(int i=0; i < numJoints; ++i){ Link* joint = body->joint(i); if(joint->isValid() && linkSelection[joint->index]){ activeJointIds.push_back(i); } } } else { activeJointIds.resize(numJoints); for(int i=0; i < numJoints; ++i){ activeJointIds[i] = i; } } int n = activeJointIds.size(); initializeSliders(n); int nColumns = numColumnsSpin.value(); bool isLabelAtLeft = labelOnLeftToggle.isChecked(); int nUnitColumns, nGridColumns; if(isLabelAtLeft){ nUnitColumns = 6; nGridColumns = nColumns * nUnitColumns; } else { nUnitColumns = 5; nGridColumns = nColumns * nUnitColumns; } int row = 0; int col = 0; for(int i=0; i < n; ++i){ SliderUnit* unit = jointSliders[i]; unit->enableSpinEntry(putSpinEntryCheck.isChecked()); unit->enableSlider(putSliderCheck.isChecked()); unit->setSlotOnValueChanged(bind(&JointSliderViewImpl::onJointSliderChanged, this, i)); int jointId = activeJointIds[i]; Link* joint = body->joint(jointId); if(jointIdToggle.isChecked()){ unit->setId(joint->jointId); } else { unit->hideId(); } if(nameToggle.isChecked()){ unit->setName(joint->name().c_str()); } else { unit->hideName(); } unit->setRange(joint->llimit, joint->ulimit); unit->setValue(joint->q); if(!isLabelAtLeft){ sliderGrid.addWidget(&unit->nameLabel, row, col, 1, nUnitColumns); sliderGrid.addWidget(&unit->idLabel, row + 1, col); attachSliderUnits(unit, row + 1, col + 1); col += nUnitColumns; if(col == nGridColumns){ col = 0; row += 2; } } else { sliderGrid.addWidget(&unit->idLabel,row, col); sliderGrid.addWidget(&unit->nameLabel,row, col + 1); attachSliderUnits(unit, row, col + 2); col += nUnitColumns; if(col == nGridColumns){ col = 0; row += 1; } } } } } void JointSliderViewImpl::attachSliderUnits(SliderUnit* unit, int row, int col) { sliderGrid.addWidget(&unit->spin, row, col); sliderGrid.addWidget(&unit->lowerLimitLabel,row, col + 1); sliderGrid.addWidget(&unit->slider, row, col + 2); sliderGrid.addWidget(&unit->upperLimitLabel,row, col + 3); } void JointSliderViewImpl::initializeSliders(int num) { int prevNum = jointSliders.size(); for(int i=0; i < prevNum; ++i){ jointSliders[i]->removeWidgesFrom(sliderGrid); } if(num > prevNum){ for(int i=prevNum; i < num; ++i){ int index = jointSliders.size(); jointSliders.push_back(new SliderUnit(this, index)); } } else if(num < prevNum){ for(int i=num; i < prevNum; ++i){ delete jointSliders[i]; } jointSliders.resize(num); } } void JointSliderViewImpl::onNumColumnsChanged(int n) { callLater(bind(&JointSliderViewImpl::updateSliderGrid, this)); } bool JointSliderViewImpl::eventFilter(QObject* object, QEvent* event) { Slider* slider = dynamic_cast(object); if(slider && (event->type() == QEvent::KeyPress)){ return onSliderKeyPressEvent(slider, static_cast(event)); } return QObject::eventFilter(object, event); } bool JointSliderViewImpl::onSliderKeyPressEvent(Slider* slider, QKeyEvent* event) { int index = slider->property("JointSliderIndex").toInt(); bool doContinue = false; switch(event->key()){ case Qt::Key_Up: focusSlider(index - 1); break; case Qt::Key_Down: focusSlider(index + 1); break; default: doContinue = true; break; } return !doContinue; } void JointSliderViewImpl::focusSlider(int index) { if(index >= 0 && index < static_cast(jointSliders.size())){ Slider& slider = jointSliders[index]->slider; slider.setFocus(Qt::OtherFocusReason); scrollArea.ensureWidgetVisible(&slider); } } void JointSliderViewImpl::onJointSliderChanged(int sliderIndex) { int jointId = activeJointIds[sliderIndex]; Link* joint = currentBodyItem->body()->joint(jointId); joint->q = jointSliders[sliderIndex]->value(); connectionOfKinematicStateChanged.block(); currentBodyItem->notifyKinematicStateChange(true); connectionOfKinematicStateChanged.unblock(); } void JointSliderViewImpl::onKinematicStateChanged() { BodyPtr body = currentBodyItem->body(); for(size_t i=0; i < activeJointIds.size(); ++i){ int jointId = activeJointIds[i]; double q = body->joint(jointId)->q; double v = jointSliders[i]->value(); if(q != v){ jointSliders[i]->setValue(q); } } } void JointSliderViewImpl::onCurrentBodyItemChanged(BodyItem* bodyItem) { currentBodyItem = bodyItem; connectionOfLinkSelectionChanged.disconnect(); if(currentBodyItem){ connectionOfLinkSelectionChanged = LinkSelectionView::mainInstance()->sigSelectionChanged(bodyItem).connect (bind(&JointSliderViewImpl::updateSliderGrid, this)); } updateSliderGrid(); enableConnectionToSigKinematicStateChanged(true); } void JointSliderViewImpl::enableConnectionToSigKinematicStateChanged(bool on) { connectionOfKinematicStateChanged.disconnect(); if(on && self->isActive() && currentBodyItem){ connectionOfKinematicStateChanged = currentBodyItem->sigKinematicStateChanged().connect( bind(&JointSliderViewImpl::onKinematicStateChanged, this)); onKinematicStateChanged(); } } bool JointSliderView::storeState(Archive& archive) { return impl->storeState(archive); } bool JointSliderViewImpl::storeState(Archive& archive) { archive.write("showAllJoints", (showAllToggle.isChecked())); archive.write("jointId", (jointIdToggle.isChecked())); archive.write("name", (nameToggle.isChecked())); archive.write("numColumns", (numColumnsSpin.value())); archive.write("spinBox", (putSpinEntryCheck.isChecked())); archive.write("slider", (putSliderCheck.isChecked())); archive.write("labelOnLeft", (labelOnLeftToggle.isChecked())); archive.writeItemId("currentBodyItem", currentBodyItem); return true; } bool JointSliderView::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool JointSliderViewImpl::restoreState(const Archive& archive) { showAllToggle.setChecked(archive.get("showAllJoints", true)); jointIdToggle.setChecked(archive.get("jointId", false)); nameToggle.setChecked(archive.get("name", true)); numColumnsSpin.setValue(archive.get("numColumns", 1)); putSpinEntryCheck.setChecked(archive.get("spinBox", true)); putSliderCheck.setChecked(archive.get("slider", true)); labelOnLeftToggle.setChecked(archive.get("labelOnLeft", true)); onCurrentBodyItemChanged(archive.findItem("currentBodyItem")); return true; } choreonoid-1.1.0+dfsg/src/BodyPlugin/JointSliderView.h000066400000000000000000000010501207742442300226570ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_JOINT_SLIDER_VIEW_H_INCLUDED #define CNOID_BODYPLUGIN_JOINT_SLIDER_VIEW_H_INCLUDED #include namespace cnoid { class JointSliderViewImpl; class JointSliderView : public cnoid::View { public: JointSliderView(); virtual ~JointSliderView(); private: JointSliderViewImpl* impl; virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/KinematicFaultChecker.cpp000066400000000000000000000435111207742442300243260ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #include "BodyItem.h" #include "BodyMotionItem.h" #include "KinematicFaultChecker.h" #include "LinkSelectionView.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { bool USE_DUPLICATED_BODY = false; KinematicFaultChecker* checkerInstance = 0; #ifdef _MSC_VER inline long lround(double x) { return static_cast((x > 0.0) ? floor(x + 0.5) : ceil(x -0.5)); } #endif } namespace cnoid { class KinematicFaultCheckerImpl : public QDialog { public: MessageView& mes; std::ostream& os; CheckBox positionCheck; DoubleSpinBox angleMarginSpin; DoubleSpinBox translationMarginSpin; CheckBox velocityCheck; QButtonGroup radioGroup; RadioButton allJointsRadio; RadioButton selectedJointsRadio; RadioButton nonSelectedJointsRadio; DoubleSpinBox velocityLimitRatioSpin; CheckBox collisionCheck; CheckBox onlyTimeBarRangeCheck; int numFaults; vector lastPosFaultFrames; vector lastVelFaultFrames; typedef std::map LastCollisionFrameMap; LastCollisionFrameMap lastCollisionFrames; double frameRate; double angleMargin; double translationMargin; double velocityLimitRatio; KinematicFaultCheckerImpl(); bool store(Archive& archive); void restore(const Archive& archive); void apply(); int checkFaults( BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os, bool checkPosition, bool checkVelocity, bool checkCollision, dynamic_bitset<> linkSelection, double beginningTime, double endingTime); void putJointPositionFault(int frame, Link* joint, std::ostream& os); void putJointVelocityFault(int frame, Link* joint, std::ostream& os); void putSelfCollision(int frame, ColdetLinkPair* linkPair, std::ostream& os); }; } void KinematicFaultChecker::initialize(ExtensionManager& ext) { if(!checkerInstance){ checkerInstance = ext.manage(new KinematicFaultChecker()); MenuManager& mm = ext.menuManager(); mm.setPath("/Tools"); mm.addItem(_("Kinematic Fault Checker")) ->sigTriggered().connect(bind(&KinematicFaultCheckerImpl::show, checkerInstance->impl)); ext.connectProjectArchiver( "KinematicFaultChecker", bind(&KinematicFaultCheckerImpl::store, checkerInstance->impl, _1), bind(&KinematicFaultCheckerImpl::restore, checkerInstance->impl, _1)); } } KinematicFaultChecker* KinematicFaultChecker::instance() { return checkerInstance; } KinematicFaultChecker::KinematicFaultChecker() { impl = new KinematicFaultCheckerImpl(); } KinematicFaultChecker::~KinematicFaultChecker() { delete impl; } KinematicFaultCheckerImpl::KinematicFaultCheckerImpl() : QDialog(MainWindow::instance()), mes(*MessageView::mainInstance()), os(mes.cout()) { setWindowTitle(_("Kinematic Fault Checker")); QVBoxLayout* vbox = new QVBoxLayout(); setLayout(vbox); QHBoxLayout* hbox = new QHBoxLayout(); positionCheck.setText(_("Joint position check")); positionCheck.setChecked(true); hbox->addWidget(&positionCheck); hbox->addSpacing(10); hbox->addWidget(new QLabel(_("Angle margin"))); angleMarginSpin.setDecimals(2); angleMarginSpin.setRange(-99.99, 99.99); angleMarginSpin.setSingleStep(0.01); hbox->addWidget(&angleMarginSpin); hbox->addWidget(new QLabel("[deg]")); hbox->addSpacing(10); hbox->addWidget(new QLabel(_("Translation margin"))); translationMarginSpin.setDecimals(4); translationMarginSpin.setRange(-9.9999, 9.9999); translationMarginSpin.setSingleStep(0.0001); hbox->addWidget(&translationMarginSpin); hbox->addWidget(new QLabel("[m]")); hbox->addStretch(); vbox->addLayout(hbox); hbox = new QHBoxLayout(); velocityCheck.setText(_("Joint velocity check")); velocityCheck.setChecked(true); hbox->addWidget(&velocityCheck); hbox->addSpacing(10); hbox->addWidget(new QLabel(_("Limit ratio"))); velocityLimitRatioSpin.setDecimals(0); velocityLimitRatioSpin.setRange(1.0, 100.0); velocityLimitRatioSpin.setValue(100.0); hbox->addWidget(&velocityLimitRatioSpin); hbox->addWidget(new QLabel("%")); hbox->addStretch(); vbox->addLayout(hbox); hbox = new QHBoxLayout(); radioGroup.addButton(&allJointsRadio); radioGroup.addButton(&selectedJointsRadio); radioGroup.addButton(&nonSelectedJointsRadio); allJointsRadio.setText(_("All joints")); allJointsRadio.setChecked(true); hbox->addWidget(&allJointsRadio); selectedJointsRadio.setText(_("Selected joints")); hbox->addWidget(&selectedJointsRadio); nonSelectedJointsRadio.setText(_("Non-selected joints")); hbox->addWidget(&nonSelectedJointsRadio); hbox->addStretch(); vbox->addLayout(hbox); vbox->addWidget(new HSeparator); hbox = new QHBoxLayout(); collisionCheck.setText(_("Self-collision check")); collisionCheck.setChecked(true); hbox->addWidget(&collisionCheck); hbox->addStretch(); vbox->addLayout(hbox); vbox->addWidget(new HSeparator); hbox = new QHBoxLayout(); onlyTimeBarRangeCheck.setText(_("Time bar's range only")); onlyTimeBarRangeCheck.setChecked(false); hbox->addWidget(&onlyTimeBarRangeCheck); hbox->addStretch(); vbox->addLayout(hbox); vbox->addWidget(new HSeparator); PushButton* applyButton = new PushButton(_("&Apply")); applyButton->setDefault(true); QDialogButtonBox* buttonBox = new QDialogButtonBox(this); buttonBox->addButton(applyButton, QDialogButtonBox::AcceptRole); applyButton->sigClicked().connect(bind(&KinematicFaultCheckerImpl::apply, this)); vbox->addWidget(buttonBox); } bool KinematicFaultCheckerImpl::store(Archive& archive) { archive.write("checkJointPositions", positionCheck.isChecked()); archive.write("angleMargin", angleMarginSpin.value()); archive.write("translationMargin", translationMarginSpin.value()); archive.write("checkJointVelocities", velocityCheck.isChecked()); archive.write("velocityLimitRatio", velocityLimitRatioSpin.value()); archive.write("targetJoints", (allJointsRadio.isChecked() ? "all" : (selectedJointsRadio.isChecked() ? "selected" : "non-selected"))); archive.write("checkSelfCollisions", collisionCheck.isChecked()); archive.write("onlyTimeBarRange", onlyTimeBarRangeCheck.isChecked()); return true; } void KinematicFaultCheckerImpl::restore(const Archive& archive) { positionCheck.setChecked(archive.get("checkJointPositions", positionCheck.isChecked())); angleMarginSpin.setValue(archive.get("angleMargin", angleMarginSpin.value())); translationMarginSpin.setValue(archive.get("translationMargin", translationMarginSpin.value())); velocityCheck.setChecked(archive.get("checkJointVelocities", velocityCheck.isChecked())); velocityLimitRatioSpin.setValue(archive.get("velocityLimitRatio", velocityLimitRatioSpin.value())); string target; if(archive.read("targetJoints", target)){ if(target == "all"){ allJointsRadio.setChecked(true); } else if(target == "selected"){ selectedJointsRadio.setChecked(true); } else if(target == "non-selected"){ nonSelectedJointsRadio.setChecked(true); } } collisionCheck.setChecked(archive.get("checkSelfCollisions", collisionCheck.isChecked())); onlyTimeBarRangeCheck.setChecked(archive.get("onlyTimeBarRange", onlyTimeBarRangeCheck.isChecked())); } void KinematicFaultCheckerImpl::apply() { bool processed = false; ItemList items = ItemTreeView::mainInstance()->selectedItems(); if(items.empty()){ mes.notify(_("No BodyMotionItems are selected.")); } else { for(size_t i=0; i < items.size(); ++i){ BodyMotionItem* motionItem = items[i]; BodyItem* bodyItem = motionItem->findOwnerItem(); if(!bodyItem){ mes.notify(str(format(_("%1% is not owned by any BodyItem. Check skiped.")) % motionItem->name())); } else { mes.putln(); mes.notify(str(format(_("Applying the Kinematic Fault Checker to %1% ...")) % motionItem->headItem()->name())); dynamic_bitset<> linkSelection; if(selectedJointsRadio.isChecked()){ linkSelection = LinkSelectionView::mainInstance()->getLinkSelection(bodyItem); } else if(nonSelectedJointsRadio.isChecked()){ linkSelection = LinkSelectionView::mainInstance()->getLinkSelection(bodyItem); linkSelection.flip(); } else { linkSelection.resize(bodyItem->body()->numLinks(), true); } double beginningTime = 0.0; double endingTime = motionItem->motion()->getTimeLength(); std::numeric_limits::max(); if(onlyTimeBarRangeCheck.isChecked()){ TimeBar* timeBar = TimeBar::instance(); beginningTime = timeBar->minTime(); endingTime = timeBar->maxTime(); } int n = checkFaults(bodyItem, motionItem, mes.cout(), positionCheck.isChecked(), velocityCheck.isChecked(), collisionCheck.isChecked(), linkSelection, beginningTime, endingTime); if(n > 0){ if(n == 1){ mes.notify(_("A fault has been detected.")); } else { mes.notify(str(format(_("%1% faults have been detected.")) % n)); } } else { mes.notify(_("No faults have been detected.")); } processed = true; } } } } /** @return Number of detected faults */ int KinematicFaultChecker::checkFaults (BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os, double beginningTime, double endingTime) { dynamic_bitset<> linkSelection(bodyItem->body()->numLinks()); linkSelection.set(); return impl->checkFaults( bodyItem, motionItem, os, true, true, true, linkSelection, beginningTime, endingTime); } int KinematicFaultCheckerImpl::checkFaults (BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os, bool checkPosition, bool checkVelocity, bool checkCollision, dynamic_bitset<> linkSelection, double beginningTime, double endingTime) { numFaults = 0; BodyPtr body = bodyItem->body(); BodyMotionPtr motion = motionItem->motion(); MultiValueSeqPtr qseq = motion->jointPosSeq();; MultiAffine3SeqPtr pseq = motion->linkPosSeq(); if((!checkPosition && !checkVelocity && !checkCollision) || body->isStaticModel() || !qseq->getNumFrames()){ return numFaults; } vector* pColdetLinkPairs; BodyItem::KinematicState orgKinematicState; if(USE_DUPLICATED_BODY){ body = body->duplicate(); pColdetLinkPairs = new vector(bodyItem->selfColdetPairs); } else { bodyItem->storeKinematicState(orgKinematicState); pColdetLinkPairs = &bodyItem->selfColdetPairs; } const int numJoints = std::min(body->numJoints(), qseq->numParts()); const int numLinks = std::min(body->numLinks(), pseq->numParts()); const int numPairs = pColdetLinkPairs->size(); frameRate = motion->frameRate(); double stepRatio2 = 2.0 / frameRate; angleMargin = radian(angleMarginSpin.value()); translationMargin = translationMarginSpin.value(); velocityLimitRatio = velocityLimitRatioSpin.value() / 100.0; int beginningFrame = std::max(0, (int)(beginningTime * frameRate)); int endingFrame = std::min((motion->numFrames() - 1), (int)lround(endingTime * frameRate)); lastPosFaultFrames.clear(); lastPosFaultFrames.resize(numJoints, std::numeric_limits::min()); lastVelFaultFrames.clear(); lastVelFaultFrames.resize(numJoints, std::numeric_limits::min()); lastCollisionFrames.clear(); if(checkCollision){ Link* root = body->rootLink(); root->p.setZero(); root->R.setIdentity(); } for(int frame = beginningFrame; frame <= endingFrame; ++frame){ int prevFrame = (frame == beginningFrame) ? beginningFrame : frame - 1; int nextFrame = (frame == endingFrame) ? endingFrame : frame + 1; for(int i=0; i < numJoints; ++i){ Link* joint = body->joint(i); double q = qseq->at(frame, i); joint->q = q; if(joint->index >= 0 && linkSelection[joint->index]){ if(checkPosition){ bool fault = false; if(joint->jointType == Link::ROTATIONAL_JOINT){ fault = (q > (joint->ulimit - angleMargin) || q < (joint->llimit + angleMargin)); } else if(joint->jointType == Link::SLIDE_JOINT){ fault = (q > (joint->ulimit - translationMargin) || q < (joint->llimit + translationMargin)); } if(fault){ putJointPositionFault(frame, joint, os); } } if(checkVelocity){ double dq = (qseq->at(nextFrame, i) - qseq->at(prevFrame, i)) / stepRatio2; joint->dq = dq; if(dq > (joint->uvlimit * velocityLimitRatio) || dq < (joint->lvlimit * velocityLimitRatio)){ putJointVelocityFault(frame, joint, os); } } } } if(checkCollision){ body->calcForwardKinematics(); for(int i=0; i < numLinks; ++i){ Link* link = body->link(i); const Affine3& p = pseq->at(frame, i); link->p = p.translation(); link->R = p.linear(); } for(int i=0; i < numPairs; ++i){ ColdetLinkPair* linkPair = (*pColdetLinkPairs)[i].get(); linkPair->updatePositions(); linkPair->detectCollisions(); if(linkPair->checkCollision()){ putSelfCollision(frame, linkPair, os); } } } } if(USE_DUPLICATED_BODY){ delete pColdetLinkPairs; } else { bodyItem->restoreKinematicState(orgKinematicState); } return numFaults; } void KinematicFaultCheckerImpl::putJointPositionFault(int frame, Link* joint, std::ostream& os) { static format f1(_("%1$7.3f [s]: Position limit over of %2% (%3% is beyond the range (%4% , %5%) with margin %6%.)")); static format f2(_("%1$7.3f [s]: Position limit over of %2% (%3% is beyond the range (%4% , %5%).)")); if(frame > lastPosFaultFrames[joint->jointId] + 1){ double q, l, u, m; if(joint->jointType == Link::ROTATIONAL_JOINT){ q = degree(joint->q); l = degree(joint->llimit); u = degree(joint->ulimit); m = degree(angleMargin); } else { q = joint->q; l = joint->llimit; u = joint->ulimit; m = translationMargin; } if(m != 0.0){ os << (f1 % (frame / frameRate) % joint->name() % q % l % u % m) << endl; } else { os << (f2 % (frame / frameRate) % joint->name() % q % l % u) << endl; } numFaults++; } lastPosFaultFrames[joint->jointId] = frame; } void KinematicFaultCheckerImpl::putJointVelocityFault(int frame, Link* joint, std::ostream& os) { static format f(_("%1$7.3f [s]: Velocity limit over of %2% (%3% is %4$.0f %% of the range (%5% , %6%).)")); if(frame > lastVelFaultFrames[joint->jointId] + 1){ double dq, l, u; if(joint->jointType == Link::ROTATIONAL_JOINT){ dq = degree(joint->dq); l = degree(joint->lvlimit); u = degree(joint->uvlimit); } else { dq = joint->dq; l = joint->lvlimit; u = joint->uvlimit; } double r = (dq < 0.0) ? (dq / l) : (dq / u); r *= 100.0; os << (f % (frame / frameRate) % joint->name() % dq % r % l % u) << endl; numFaults++; } lastVelFaultFrames[joint->jointId] = frame; } void KinematicFaultCheckerImpl::putSelfCollision(int frame, ColdetLinkPair* linkPair, std::ostream& os) { static format f(_("%1$7.3f [s]: Collision between %2% and %3%")); bool putMessage = false; LastCollisionFrameMap::iterator p = lastCollisionFrames.find(linkPair); if(p == lastCollisionFrames.end()){ putMessage = true; lastCollisionFrames[linkPair] = frame; } else { if(frame > p->second + 1){ putMessage = true; } p->second = frame; } if(putMessage){ os << (f % (frame / frameRate) % linkPair->link(0)->name() % linkPair->link(1)->name()) << endl; numFaults++; } } choreonoid-1.1.0+dfsg/src/BodyPlugin/KinematicFaultChecker.h000066400000000000000000000015561207742442300237760ustar00rootroot00000000000000/** @author Shin'ichiro NAKAOKA */ #ifndef CNOID_BODYPLUGIN_KINEMATIC_FAULT_CHECKER_H_INCLUDED #define CNOID_BODYPLUGIN_KINEMATIC_FAULT_CHECKER_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { class ExtensionManager; class BodyItem; class BodyMotionItem; class KinematicFaultCheckerImpl; class CNOID_EXPORT KinematicFaultChecker { public: static void initialize(ExtensionManager& ext); static KinematicFaultChecker* instance(); KinematicFaultChecker(); virtual ~KinematicFaultChecker(); int checkFaults( BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os, double beginningTime = 0.0, double endingTime = std::numeric_limits::max()); private: KinematicFaultCheckerImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/KinematicsBar.cpp000066400000000000000000000172441207742442300226610ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "KinematicsBar.h" #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const char* modeSymbol[] = { "AUTO", "FK", "IK" }; } namespace cnoid { class KinematicsBarSetupDialog : public QDialog { public: DoubleSpinBox snapDistanceSpin; SpinBox snapAngleSpin; DoubleSpinBox penetrationBlockDepthSpin; CheckBox lazyCollisionDetectionModeCheck; PushButton okButton; KinematicsBarSetupDialog() : QDialog(MainWindow::instance()) { setWindowTitle(_("Kinematics Operation Setup")); QVBoxLayout* vbox = new QVBoxLayout(); setLayout(vbox); QHBoxLayout* hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(_("Snap thresholds:"))); hbox->addSpacing(10); hbox->addWidget(new QLabel(_("distance"))); snapDistanceSpin.setAlignment(Qt::AlignCenter); snapDistanceSpin.setDecimals(3); snapDistanceSpin.setRange(0.0, 0.999); snapDistanceSpin.setSingleStep(0.001); snapDistanceSpin.setValue(0.025); hbox->addWidget(&snapDistanceSpin); hbox->addWidget(new QLabel(_("[m]"))); hbox->addSpacing(5); hbox->addWidget(new QLabel(_("angle"))); snapAngleSpin.setAlignment(Qt::AlignCenter); snapAngleSpin.setRange(0, 90); snapAngleSpin.setValue(30); hbox->addWidget(&snapAngleSpin); hbox->addWidget(new QLabel(_("[deg]"))); vbox->addLayout(hbox); hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(_("Penetration block depth"))); penetrationBlockDepthSpin.setAlignment(Qt::AlignCenter); penetrationBlockDepthSpin.setDecimals(4); penetrationBlockDepthSpin.setRange(0.0, 0.0099); penetrationBlockDepthSpin.setSingleStep(0.0001); penetrationBlockDepthSpin.setValue(0.0005); hbox->addWidget(&penetrationBlockDepthSpin); hbox->addWidget(new QLabel(_("[m]"))); vbox->addLayout(hbox); hbox = new QHBoxLayout(); lazyCollisionDetectionModeCheck.setText(_("Lazy collision detection mode")); lazyCollisionDetectionModeCheck.setChecked(true); hbox->addWidget(&lazyCollisionDetectionModeCheck); vbox->addLayout(hbox); hbox = new QHBoxLayout(); okButton.setText(_("OK")); okButton.setDefault(true); hbox->addWidget(&okButton); vbox->addLayout(hbox); } void storeState(Archive& archive){ archive.write("snapDistance", snapDistanceSpin.value()); archive.write("penetrationBlockDepth", penetrationBlockDepthSpin.value()); archive.write("lazyCollisionDetectionMode", lazyCollisionDetectionModeCheck.isChecked()); } void restoreState(const Archive& archive){ snapDistanceSpin.setValue(archive.get("snapDistance", snapDistanceSpin.value())); penetrationBlockDepthSpin.setValue(archive.get("penetrationBlockDepth", penetrationBlockDepthSpin.value())); lazyCollisionDetectionModeCheck.setChecked(archive.get("lazyCollisionDetectionMode", lazyCollisionDetectionModeCheck.isChecked())); } /* virtual void on_response(int id) { hide(); } */ }; } KinematicsBar* KinematicsBar::instance() { static KinematicsBar* instance = new KinematicsBar(); return instance; } KinematicsBar::KinematicsBar() : ToolBar(N_("KinematicsBar")) { setup = new KinematicsBarSetupDialog(); addSeparator(2); fkModeRadio = addRadioButton(QIcon(":/Body/icons/fk.png"), _("Forward kinematics mode")); autoModeRadio = addRadioButton(QIcon(":/Body/icons/fkik.png"), _("Preset kinematics mode")); ikModeRadio = addRadioButton(QIcon(":/Body/icons/ik.png"), _("Inverse kinematics mode")); autoModeRadio->setChecked(true); addSeparator(2); attitudeToggle = addToggleButton(QIcon(":/Body/icons/rotation.png"), _("Enable link orientation editing")); attitudeToggle->setChecked(false); /* footSnapToggle = addToggleButton(xpmfootsnapon, _("Snap foot to the floor")); footSnapToggle->setChecked(true); */ /* jointPositionLimitToggle = addToggleButton( xpmlimitangle, _("Limit joint positions within the movable range specifications")); jointPositionLimitToggle->setChecked(true); */ penetrationBlockToggle = addToggleButton(QIcon(":/Body/icons/block.png"), _("Penetration block mode")); penetrationBlockToggle->setChecked(true); collisionLinkHighlightToggle = addToggleButton(QIcon(":/Body/icons/collisionoutline.png"), _("Highlight colliding links")); collisionLinkHighlightToggle->setChecked(false); collisionLinkHighlightToggle->sigToggled().connect(bind(&KinematicsBar::onCollisionVisualizationChanged, this)); addButton(QIcon(":/Base/icons/setup.png"))->sigClicked().connect(bind(&KinematicsBarSetupDialog::show, setup)); setup->lazyCollisionDetectionModeCheck.sigToggled().connect( bind(&KinematicsBar::onLazyCollisionDetectionModeToggled, this)); onLazyCollisionDetectionModeToggled(); } KinematicsBar::~KinematicsBar() { } int KinematicsBar::mode() const { if(fkModeRadio->isChecked()){ return FK_MODE; } else if(ikModeRadio->isChecked()){ return IK_MODE; } return AUTO_MODE; } void KinematicsBar::getSnapThresholds(double& distance, double& angle) const { distance = setup->snapDistanceSpin.value(); angle = radian(setup->snapAngleSpin.value()); } double KinematicsBar::penetrationBlockDepth() const { return setup->penetrationBlockDepthSpin.value(); } void KinematicsBar::onCollisionVisualizationChanged() { sigCollisionVisualizationChanged_(); } void KinematicsBar::onLazyCollisionDetectionModeToggled() { if(setup->lazyCollisionDetectionModeCheck.isChecked()){ collisionDetectionPriority_ = IDLE_PRIORITY_NORMAL; } else { collisionDetectionPriority_ = IDLE_PRIORITY_HIGH; } } bool KinematicsBar::storeState(Archive& archive) { archive.write("mode", modeSymbol[mode()]); archive.write("attitude", attitudeToggle->isChecked()); archive.write("penetrationBlock", penetrationBlockToggle->isChecked()); //archive.write("footSnap", footSnapToggle->isChecked()); archive.write("collisionLinkHighlight", collisionLinkHighlightToggle->isChecked()); setup->storeState(archive); return true; } bool KinematicsBar::restoreState(const Archive& archive) { string storedModeSymbol = archive.get("mode", "AUTO"); if(storedModeSymbol == "FK"){ fkModeRadio->setChecked(true); } else if(storedModeSymbol == "IK"){ ikModeRadio->setChecked(true); } else { autoModeRadio->setChecked(true); fkModeRadio->setChecked(false); ikModeRadio->setChecked(false); } attitudeToggle->setChecked(archive.get("attitude", attitudeToggle->isChecked())); penetrationBlockToggle->setChecked(archive.get("penetrationBlock", penetrationBlockToggle->isChecked())); //footSnapToggle->setChecked(archive.get("footSnap", footSnapToggle->isChecked())); collisionLinkHighlightToggle->setChecked(archive.get("collisionLinkHighlight", collisionLinkHighlightToggle->isChecked())); setup->restoreState(archive); return true; } choreonoid-1.1.0+dfsg/src/BodyPlugin/KinematicsBar.h000066400000000000000000000040521207742442300223170ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_KINEMATICS_BAR_H_INCLUDED #define CNOID_BODYPLUGIN_KINEMATICS_BAR_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class KinematicsBarSetupDialog; class CNOID_EXPORT KinematicsBar : public ToolBar { public: static KinematicsBar* instance(); virtual ~KinematicsBar(); enum Mode { AUTO_MODE, FK_MODE, IK_MODE }; int mode() const; bool isAttitudeMode() const { return attitudeToggle->isChecked(); } bool isFootSnapMode() const { return footSnapToggle->isChecked(); } void getSnapThresholds(double& distance, double& angle) const; bool isJointPositionLimitMode() const { return jointPositionLimitToggle->isChecked(); } bool isPenetrationBlockMode() const { return penetrationBlockToggle->isChecked(); } double penetrationBlockDepth() const; bool isCollisionLinkHighlihtMode() const { return collisionLinkHighlightToggle->isChecked(); } int collisionDetectionPriority() const { return collisionDetectionPriority_; } SignalProxy< boost::signal > sigCollisionVisualizationChanged() { return sigCollisionVisualizationChanged_; } protected: virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); private: ToolButton* autoModeRadio; ToolButton* fkModeRadio; ToolButton* ikModeRadio; ToolButton* attitudeToggle; ToolButton* footSnapToggle; ToolButton* jointPositionLimitToggle; ToolButton* penetrationBlockToggle; ToolButton* collisionLinkHighlightToggle; int collisionDetectionPriority_; boost::signal sigCollisionVisualizationChanged_; KinematicsBarSetupDialog* setup; KinematicsBar(); void onCollisionVisualizationChanged(); void onLazyCollisionDetectionModeToggled(); }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/KinematicsSimulatorItem.cpp000066400000000000000000000304751207742442300247540ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "KinematicsSimulatorItem.h" #include "WorldItem.h" #include "BodyItem.h" #include "BodyMotionItem.h" #include #include #include #include #include "gettext.h" using namespace std; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; class BodyUnit { public: int frame; double frameRate; BodyPtr body; MultiValueSeqPtr qseqRef; Vector3SeqPtr zmpSeq; MultiValueSeqPtr qseqResultBuf; MultiValueSeqPtr qseqResult; MultiAffine3SeqItemPtr rootResultItem; MultiAffine3SeqPtr rootResultBuf; MultiAffine3SeqPtr rootResult; Link* baseLink; LinkTraverse fkTraverse; vector footLinks; vector footLinkOriginHeights; bool initialize(BodyItemPtr bodyItem); bool updatePosition(int newFrame); void putResultToBuf(); void setBaseLink(Link* link, double originHeight); void setBaseLinkByZmp(int frame); bool flushBuf(); }; } namespace cnoid { class KSIImpl { public: KSIImpl(KinematicsSimulatorItem* self); KSIImpl(KinematicsSimulatorItem* self, const KSIImpl& org); bool doStartSimulation(); bool setupBodies(); void addBody(BodyItemPtr bodyItem); bool doStepSimulation(); double doFlushResults(); KinematicsSimulatorItem* self; vector bodyUnits; vector bodyUnitsToPutResult; vector bodyUnitsToNotifyUpdate; int currentFrame; int frameAtLastBufferWriting; double mainFrameRate; }; } void cnoid::initializeKinematicsSimulatorItem(ExtensionManager& ext) { ext.itemManager().registerClass(N_("KinematicsSimulatorItem")); ext.itemManager().addCreationPanel(); } bool BodyUnit::initialize(BodyItemPtr bodyItem) { if(TRACE_FUNCTIONS){ cout << "BodyUnit::initialize()" << endl; } if(bodyItem->body()->isStaticModel()){ return false; } frame = 0; body = bodyItem->body()->duplicate(); baseLink = 0; ItemList motionItems = bodyItem->getSubItems(); if(!motionItems.empty()){ BodyMotionItemPtr motionItem; // prefer the first checked item for(size_t i=0; i < motionItems.size(); ++i){ if(ItemTreeView::mainInstance()->isItemChecked(motionItems[i])){ motionItem = motionItems[i]; break; } } if(!motionItem){ motionItem = motionItems[0]; } frameRate = motionItem->motion()->frameRate(); qseqRef = motionItem->jointPosSeq(); if(qseqResult){ qseqResultBuf.reset(new MultiValueSeq(qseqResult->numParts(), 0, frameRate)); } if(motionItem->hasRelativeZmpSeqItem()){ zmpSeq = motionItem->relativeZmpSeq(); } rootResultItem = motionItem->linkPosSeqItem(); rootResultBuf.reset(new MultiAffine3Seq(1, 0, frameRate)); rootResult = motionItem->linkPosSeq(); rootResult->setFrameRate(frameRate); rootResult->setDimension(1, 1); } const YamlMapping& info = *bodyItem->body()->info(); const YamlSequence& footLinkInfos = *info.findSequence("footLinks"); if(footLinkInfos.isValid()){ for(int i=0; i < footLinkInfos.size(); ++i){ const YamlMapping& footLinkInfo = *footLinkInfos[i].toMapping(); Link* link = body->link(footLinkInfo["link"].toString()); Vector3 soleCenter; if(link && read(footLinkInfo, "soleCenter", soleCenter)){ footLinks.push_back(link); footLinkOriginHeights.push_back(-soleCenter[2]); } } } if(!footLinks.empty()){ if(zmpSeq){ setBaseLinkByZmp(0); } else { if(bodyItem->currentBaseLink()){ int currentBaseLinkIndex = bodyItem->currentBaseLink()->index; for(size_t i=0; i < footLinks.size(); ++i){ Link* footLink = footLinks[i]; if(footLink->index == currentBaseLinkIndex){ setBaseLink(footLink, footLinkOriginHeights[i]); } } } else{ setBaseLink(footLinks[0], footLinkOriginHeights[0]); } } } if(!baseLink){ if(bodyItem->currentBaseLink()){ baseLink = body->link(bodyItem->currentBaseLink()->index); } else { baseLink = body->rootLink(); } fkTraverse.find(baseLink, true, true); } if(rootResult){ Link* rootLink = body->rootLink(); Affine3& initialPos = rootResult->at(0, 0); initialPos.translation() = rootLink->p; initialPos.linear() = rootLink->R; } return (qseqRef != 0); } void BodyUnit::setBaseLink(Link* link, double originHeight) { if(TRACE_FUNCTIONS){ cout << "BodyUnit::setBaseLink()" << endl; } baseLink = link; link->p[2] = originHeight; Matrix3& R = link->R; Matrix3::ColXpr x = R.col(0); Matrix3::ColXpr y = R.col(1); Matrix3::ColXpr z = R.col(2); z.normalize(); y = z.cross(x).normalized(); x = y.cross(z); fkTraverse.find(link, true, true); fkTraverse.calcForwardKinematics(); } void BodyUnit::setBaseLinkByZmp(int frame) { Link* root = body->rootLink(); const Vector3 zmp = root->R * zmpSeq->at(frame) + root->p; int footLinkOnZmpId = -1; double l2min = std::numeric_limits::max(); for(size_t i=0; i < footLinks.size(); ++i){ double l2 = (footLinks[i]->p - zmp).squaredNorm(); if(l2 < l2min){ footLinkOnZmpId = i; l2min = l2; } } if(footLinkOnZmpId >= 0 && footLinks[footLinkOnZmpId] != baseLink){ setBaseLink(footLinks[footLinkOnZmpId], footLinkOriginHeights[footLinkOnZmpId]); } } bool BodyUnit::updatePosition(int newFrame) { if(TRACE_FUNCTIONS){ cout << "BodyUnit::updatePosition()" << endl; } if(newFrame >= qseqRef->numFrames()){ return false; } frame = newFrame; const int n = std::min(body->numJoints(), qseqRef->numParts()); MultiValueSeq::View qRef = qseqRef->frame(frame); for(int i=0; i < n; ++i){ body->joint(i)->q = qRef[i]; } fkTraverse.calcForwardKinematics(); if(zmpSeq){ setBaseLinkByZmp(frame); } else { for(size_t i=0; i < footLinks.size(); ++i){ Link* link = footLinks[i]; if(link != baseLink){ if(link->p[2] < footLinkOriginHeights[i]){ setBaseLink(link, footLinkOriginHeights[i]); } } } } return true; } void BodyUnit::putResultToBuf() { if(TRACE_FUNCTIONS){ cout << "BodyUnit::putResultToBuf()" << endl; } const int bufFrame = rootResultBuf->numFrames(); if(qseqResultBuf){ qseqResultBuf->setNumFrames(bufFrame + 1); MultiValueSeq::View qResultBuf = qseqResultBuf->frame(bufFrame); const int n = qResultBuf.size(); for(int i=0; i < n; ++i){ qResultBuf[i] = body->joint(i)->q; } } rootResultBuf->setNumFrames(bufFrame + 1); Link* rootLink = body->rootLink(); Affine3& pos = rootResultBuf->at(bufFrame, 0); pos.translation() = rootLink->p; pos.linear() = rootLink->R; } bool BodyUnit::flushBuf() { if(TRACE_FUNCTIONS){ cout << "BodyUnit::flushBuf()" << endl; } const int frame = rootResult->numFrames(); const int numBufFrames = rootResultBuf->numFrames(); if(numBufFrames > 0){ if(qseqResult){ const int n = body->numJoints(); qseqResult->setNumFrames(frame + numBufFrames); for(int i=0; i < numBufFrames; ++i){ MultiValueSeq::View qResult = qseqResult->frame(frame + i); MultiValueSeq::View qResultBuf = qseqResultBuf->frame(i); for(int j=0; j < n; ++j){ qResult[j] = qResultBuf[j]; } } qseqResultBuf->setNumFrames(0); } rootResult->setNumFrames(frame + numBufFrames); for(int i=0; i < numBufFrames; ++i){ rootResult->at(frame + i, 0) = rootResultBuf->at(i, 0); } rootResultBuf->setNumFrames(0); } return (numBufFrames > 0); } KinematicsSimulatorItem::KinematicsSimulatorItem() { impl = new KSIImpl(this); } KSIImpl::KSIImpl(KinematicsSimulatorItem* self) : self(self) { } KinematicsSimulatorItem::KinematicsSimulatorItem(const KinematicsSimulatorItem& org) : SimulatorItem(org), impl(new KSIImpl(this, *org.impl)) { } KSIImpl::KSIImpl(KinematicsSimulatorItem* self, const KSIImpl& org) : self(self) { } KinematicsSimulatorItem::~KinematicsSimulatorItem() { delete impl; } ItemPtr KinematicsSimulatorItem::doDuplicate() const { return new KinematicsSimulatorItem(*this); } QWidget* KinematicsSimulatorItem::settingPanel() { return 0; } bool KinematicsSimulatorItem::doStartSimulation() { return impl->doStartSimulation(); } bool KSIImpl::doStartSimulation() { if(TRACE_FUNCTIONS){ cout << "BodyUnit::doStartSimulation()" << endl; } bool result = setupBodies(); if(result){ currentFrame = 0; frameAtLastBufferWriting = 0; } return result; } bool KSIImpl::setupBodies() { if(TRACE_FUNCTIONS){ cout << "KSIImpl::setupBodies()" << endl; } mainFrameRate = 0; bodyUnits.clear(); WorldItemPtr worldItem = self->findOwnerItem(); if(worldItem){ ItemList bodyItems = worldItem->getBodyItems(); for(size_t i=0; i < bodyItems.size(); ++i){ BodyUnit unit; if(unit.initialize(bodyItems[i])){ bodyUnits.push_back(unit); if(unit.frameRate > mainFrameRate){ mainFrameRate = unit.frameRate; } } } } return (!bodyUnits.empty() && mainFrameRate > 0); } bool KinematicsSimulatorItem::doStepSimulation() { return impl->doStepSimulation(); } bool KSIImpl::doStepSimulation() { if(TRACE_FUNCTIONS){ cout << "KSIImpl::doStepSimulation()" << endl; } currentFrame++; bodyUnitsToPutResult.clear(); for(size_t i=0; i < bodyUnits.size(); ++i){ BodyUnit& unit = bodyUnits[i]; int frame = static_cast(currentFrame * unit.frameRate / mainFrameRate); if(frame > unit.frame){ if(unit.updatePosition(frame)){ bodyUnitsToPutResult.push_back(&unit); } } } bool doContinue = !bodyUnitsToPutResult.empty(); if(doContinue){ self->lockResults(); for(size_t i=0; i < bodyUnitsToPutResult.size(); ++i){ bodyUnitsToPutResult[i]->putResultToBuf(); } frameAtLastBufferWriting = currentFrame; self->requestToFlushResults(); self->unlockResults(); } return doContinue; } double KinematicsSimulatorItem::doFlushResults() { return impl->doFlushResults(); } double KSIImpl::doFlushResults() { if(TRACE_FUNCTIONS){ cout << "KSIImpl::doFlushResults()" << endl; } bodyUnitsToNotifyUpdate.clear(); self->lockResults(); for(size_t i=0; i < bodyUnits.size(); ++i){ if(bodyUnits[i].flushBuf()){ bodyUnitsToNotifyUpdate.push_back(&bodyUnits[i]); } } int frame = frameAtLastBufferWriting; self->unlockResults(); for(size_t i=0; i < bodyUnitsToNotifyUpdate.size(); ++i){ bodyUnitsToNotifyUpdate[i]->rootResultItem->notifyUpdate(); } return (frame / mainFrameRate); } double KinematicsSimulatorItem::doStopSimulation() { if(TRACE_FUNCTIONS){ cout << "KinematicsSimulatorItem::doStopSimulation()" << endl; } return (impl->currentFrame / impl->mainFrameRate); } bool KinematicsSimulatorItem::store(Archive& archive) { return true; } bool KinematicsSimulatorItem::restore(const Archive& archive) { return true; } choreonoid-1.1.0+dfsg/src/BodyPlugin/KinematicsSimulatorItem.h000066400000000000000000000021041207742442300244050ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_KINEMATICS_SIMULATOR_ITEM_H_INCLUDED #define CNOID_BODYPLUGIN_KINEMATICS_SIMULATOR_ITEM_H_INCLUDED #include "SimulatorItem.h" namespace cnoid { class KSIImpl; class KinematicsSimulatorItem : public SimulatorItem { public: KinematicsSimulatorItem(); KinematicsSimulatorItem(const KinematicsSimulatorItem& org); virtual ~KinematicsSimulatorItem(); protected: virtual QWidget* settingPanel(); virtual bool doStartSimulation(); virtual bool doStepSimulation(); virtual double doFlushResults(); virtual double doStopSimulation(); virtual ItemPtr doDuplicate() const; virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); private: KSIImpl* impl; friend class KSIImpl; }; typedef boost::intrusive_ptr KinematicsSimulatorItemPtr; void initializeKinematicsSimulatorItem(ExtensionManager& ext); } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/LinkSelectionView.cpp000066400000000000000000000047321207742442300235410ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "LinkSelectionView.h" #include "LinkTreeWidget.h" #include "BodyBar.h" #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } namespace cnoid { class LinkSelectionViewImpl { public: LinkSelectionViewImpl(LinkSelectionView* self); LinkTreeWidget linkTreeWidget; }; } namespace { LinkSelectionView* mainLinkSelectionView = 0; } LinkSelectionView* LinkSelectionView::mainInstance() { assert(mainLinkSelectionView); return mainLinkSelectionView; } LinkSelectionView::LinkSelectionView() { impl = new LinkSelectionViewImpl(this); if(!mainLinkSelectionView){ mainLinkSelectionView = this; } } LinkSelectionViewImpl::LinkSelectionViewImpl(LinkSelectionView* self) { self->setName(N_("Links")); self->setDefaultLayoutArea(View::LEFT_BOTTOM); linkTreeWidget.setFrameShape(QFrame::NoFrame); linkTreeWidget.enableCache(true); linkTreeWidget.enableArchiveOfCurrentBodyItem(true); linkTreeWidget.setListingMode(LinkTreeWidget::LINK_LIST); QVBoxLayout* vbox = new QVBoxLayout(); vbox->setSpacing(0); vbox->addWidget(linkTreeWidget.listingModeCombo()); vbox->addWidget(&linkTreeWidget); self->setLayout(vbox); BodyBar::instance()->sigCurrentBodyItemChanged().connect( bind(&LinkTreeWidget::setBodyItem, &linkTreeWidget, _1)); } LinkSelectionView::~LinkSelectionView() { delete impl; } SignalProxy< boost::signal > LinkSelectionView::sigSelectionChanged(BodyItemPtr bodyItem) { return impl->linkTreeWidget.sigSelectionChanged(bodyItem); } const std::vector& LinkSelectionView::getSelectedLinkIndices(BodyItemPtr bodyItem) { return impl->linkTreeWidget.getSelectedLinkIndices(bodyItem); } const boost::dynamic_bitset<>& LinkSelectionView::getLinkSelection(BodyItemPtr bodyItem) { return impl->linkTreeWidget.getLinkSelection(bodyItem); } bool LinkSelectionView::makeSingleSelection(BodyItemPtr bodyItem, int linkIndex) { return impl->linkTreeWidget.makeSingleSelection(bodyItem, linkIndex); } bool LinkSelectionView::storeState(Archive& archive) { return impl->linkTreeWidget.storeState(archive); } bool LinkSelectionView::restoreState(const Archive& archive) { return impl->linkTreeWidget.restoreState(archive); } choreonoid-1.1.0+dfsg/src/BodyPlugin/LinkSelectionView.h000066400000000000000000000021501207742442300231760ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_LINK_SELECTION_VIEW_H_INCLUDED #define CNOID_BODYPLUGIN_LINK_SELECTION_VIEW_H_INCLUDED #include "BodyItem.h" #include #include #include #include "exportdecl.h" namespace cnoid { class LinkSelectionViewImpl; class CNOID_EXPORT LinkSelectionView : public cnoid::View, public boost::signals::trackable { public: static LinkSelectionView* mainInstance(); LinkSelectionView(); virtual ~LinkSelectionView(); SignalProxy< boost::signal > sigSelectionChanged(BodyItemPtr bodyItem); const std::vector& getSelectedLinkIndices(BodyItemPtr bodyItem); const boost::dynamic_bitset<>& getLinkSelection(BodyItemPtr bodyItem); bool makeSingleSelection(BodyItemPtr bodyItem, int linkIndex); virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); private: LinkSelectionViewImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/LinkTreeWidget.cpp000066400000000000000000000776311207742442300230340ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "LinkTreeWidget.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } namespace cnoid { class LinkTreeWidgetImpl { public: LinkTreeWidgetImpl(LinkTreeWidget* self); ~LinkTreeWidgetImpl(); LinkTreeWidget* self; struct ColumnInfo { LinkTreeWidget::ColumnDataFunction dataFunction; LinkTreeWidget::ColumnSetDataFunction setDataFunction; LinkTreeWidget::ColumnWidgetFunction widgetFunction; }; vector columnInfos; QTreeWidgetItem* headerItem; int nameColumn; int jointIdColumn; int rowIndexCounter; int itemWidgetWidthAdjustment; vector customRows; LinkTreeWidget::ListingMode listingMode; ComboBox listingModeCombo; Menu popupMenu; // This must be defined before popupMenuManager MenuManager popupMenuManager; boost::signal sigUpdateRequest; boost::signal sigItemChanged; boost::signal sigSelectionChanged; int defaultExpansionLevel; bool isCacheEnabled; bool isArchiveOfCurrentBodyItemEnabled; class BodyItemInfo : public Referenced { public: bool isRestoringTreeStateNeeded; dynamic_bitset<> selection; vector selectedLinkIndices; boost::signal sigSelectionChanged; bool needTreeExpansionUpdate; dynamic_bitset<> linkExpansions; std::set expandedParts; signals::connection detachedFromRootConnection; BodyItemInfo() { isRestoringTreeStateNeeded = true; } ~BodyItemInfo() { detachedFromRootConnection.disconnect(); } void setNumLinks(int n, bool doInitialize) { if(doInitialize){ selection.reset(); linkExpansions.set(); expandedParts.clear(); } selection.resize(n, false); linkExpansions.resize(n, true); } }; typedef intrusive_ptr BodyItemInfoPtr; typedef map BodyItemInfoMap; BodyItemInfoMap bodyItemInfoCache; vector linkIndexToItemMap; BodyItemPtr currentBodyItem; BodyItemInfoPtr currentBodyItemInfo; boost::signal dummySigSelectionChanged; // never emitted vector emptyLinkIndices; dynamic_bitset<> emptySelection; void initialize(); void enableCache(bool on); BodyItemInfoPtr getBodyItemInfo(BodyItemPtr bodyItem); void onBodyItemDetachedFromRoot(BodyItem* bodyItem); void clearTreeItems(); void setCurrentBodyItem(BodyItemPtr bodyItem, bool forceTreeUpdate); void restoreTreeState(); void restoreSubTreeState(QTreeWidgetItem* item); void restoreTreeStateSub(QTreeWidgetItem* parentItem); void addChild(LinkTreeItem* parentItem, LinkTreeItem* item); void addChild(LinkTreeItem* item); void setLinkTree(Link* link, bool onlyJoints); void setLinkTreeSub(Link* link, Link* parentLink, LinkTreeItem* parentItem, bool onlyJoints); void setJointList(const BodyPtr& body); void setLinkList(const BodyPtr& body); void setLinkGroupTree(const BodyItemPtr bodyItem); void setLinkGroupTreeSub(LinkTreeItem* parentItem, const LinkGroupPtr& linkGroup, const BodyPtr& body); void addCustomRows(); void onListingModeChanged(int index); void onSelectionChanged(); boost::signal& sigSelectionChangedOf(BodyItemPtr bodyItem); const std::vector& getSelectedLinkIndices(BodyItemPtr bodyItem); const boost::dynamic_bitset<>& getLinkSelection(BodyItemPtr bodyItem); void onCustomContextMenuRequested(const QPoint& pos); void setExpansionState(const LinkTreeItem* item, bool on); void onItemExpanded(QTreeWidgetItem* treeWidgetItem); void onItemCollapsed(QTreeWidgetItem* treeWidgetItem); bool makeSingleSelection(BodyItemPtr& bodyItem, int linkIndex); bool storeState(Archive& archive); bool restoreState(const Archive& archive); }; } namespace { void jointIdData(const LinkTreeItem* item, int role, QVariant& out_value) { const Link* link = item->link(); if(role == Qt::DisplayRole && link && (link->jointId >= 0)){ out_value = link->jointId; } else if(role == Qt::TextAlignmentRole){ out_value = Qt::AlignHCenter; } } void nameData(const LinkTreeItem* item, int role, QVariant& out_value) { if(role == Qt::DisplayRole){ out_value = item->name().c_str(); } } } LinkTreeItem::LinkTreeItem(const std::string& name) : name_(name) { rowIndex_ = -1; link_ = 0; isLinkGroup_ = true; } LinkTreeItem::LinkTreeItem(Link* link, LinkTreeWidgetImpl* treeImpl) : name_(link->name()) { rowIndex_ = -1; link_ = link; isLinkGroup_ = false; treeImpl->linkIndexToItemMap[link->index] = this; } LinkTreeItem::LinkTreeItem(LinkGroup* linkGroup, LinkTreeWidgetImpl* treeImpl) : name_(linkGroup->name()) { rowIndex_ = -1; link_ = 0; isLinkGroup_ = true; } QVariant LinkTreeItem::data(int column, int role) const { QVariant value; LinkTreeWidget* tree = static_cast(treeWidget()); LinkTreeWidget::ColumnDataFunction& func = tree->impl->columnInfos[column].dataFunction; if(!func.empty()){ func(this, role, value); } if(value.isValid()){ return value; } return QTreeWidgetItem::data(column, role); } void LinkTreeItem::setData(int column, int role, const QVariant& value) { LinkTreeWidget* tree = static_cast(treeWidget()); LinkTreeWidget::ColumnSetDataFunction& func = tree->impl->columnInfos[column].setDataFunction; if(!func.empty()){ func(this, role, value); } return QTreeWidgetItem::setData(column, role, value); } LinkTreeWidget::LinkTreeWidget(QWidget* parent) : TreeWidget(parent) { impl = new LinkTreeWidgetImpl(this); impl->initialize(); } LinkTreeWidgetImpl::LinkTreeWidgetImpl(LinkTreeWidget* self) : self(self), popupMenuManager(&popupMenu) { } void LinkTreeWidgetImpl::initialize() { rowIndexCounter = 0; defaultExpansionLevel = std::numeric_limits::max(); isCacheEnabled = false; isArchiveOfCurrentBodyItemEnabled = false; itemWidgetWidthAdjustment = 0; headerItem = new QTreeWidgetItem; QHeaderView* header = self->header(); header->setStretchLastSection(false); QObject::connect(header, SIGNAL(sectionResized(int, int, int)), self, SLOT(onHeaderSectionResized())); self->setHeaderItem(headerItem); self->setSelectionMode(QAbstractItemView::ExtendedSelection); self->setIndentation(12); self->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); self->setVerticalScrollMode(QAbstractItemView::ScrollPerPixel); self->setHorizontalScrollMode(QAbstractItemView::ScrollPerPixel); nameColumn = self->addColumn(_("link")); header->setResizeMode(nameColumn, QHeaderView::Stretch); self->setColumnDataFunction(nameColumn, &nameData); jointIdColumn = self->addColumn(_("id")); self->setColumnDataFunction(jointIdColumn, &jointIdData); header->setResizeMode(jointIdColumn, QHeaderView::ResizeToContents); self->moveVisualColumnIndex(jointIdColumn, 0); QObject::connect(self, SIGNAL(itemChanged(QTreeWidgetItem*, int)), self, SLOT(onItemChanged(QTreeWidgetItem*, int))); QObject::connect(self, SIGNAL(itemExpanded(QTreeWidgetItem*)), self, SLOT(onItemExpanded(QTreeWidgetItem*))); QObject::connect(self, SIGNAL(itemCollapsed(QTreeWidgetItem*)), self, SLOT(onItemCollapsed(QTreeWidgetItem*))); QObject::connect(self, SIGNAL(itemSelectionChanged()), self, SLOT(onSelectionChanged())); // The order of the following labes must correspond to that of ListingMode listingModeCombo.enableI18n(CNOID_GETTEXT_DOMAIN_NAME); listingModeCombo.addI18nItem(N_("link list")); listingModeCombo.addI18nItem(N_("link tree")); listingModeCombo.addI18nItem(N_("joint list")); listingModeCombo.addI18nItem(N_("joint tree")); listingModeCombo.addI18nItem(N_("part tree")); listingMode = LinkTreeWidget::LINK_LIST; listingModeCombo.setCurrentIndex(listingMode); listingModeCombo.sigCurrentIndexChanged().connect( bind(&LinkTreeWidgetImpl::onListingModeChanged, this, _1)); } LinkTreeWidget::~LinkTreeWidget() { delete impl; } LinkTreeWidgetImpl::~LinkTreeWidgetImpl() { for(size_t i=0; i < customRows.size(); ++i){ delete customRows[i]; } } void LinkTreeWidget::setDefaultExpansionLevel(int level) { impl->defaultExpansionLevel = level; } void LinkTreeWidget::enableCache(bool on) { impl->enableCache(on); } void LinkTreeWidgetImpl::enableCache(bool on) { isCacheEnabled = on; if(!isCacheEnabled){ bodyItemInfoCache.clear(); } } int LinkTreeWidget::nameColumn() { return impl->nameColumn; } int LinkTreeWidget::jointIdColumn() { return impl->jointIdColumn; } int LinkTreeWidget::addColumn() { int column = impl->columnInfos.size(); impl->columnInfos.push_back(LinkTreeWidgetImpl::ColumnInfo()); setColumnCount(impl->columnInfos.size()); return column; } int LinkTreeWidget::addColumn(const QString& headerText) { int column = addColumn(); impl->headerItem->setText(column, headerText); return column; } void LinkTreeWidget::moveVisualColumnIndex(int column, int visualIndex) { header()->moveSection(header()->visualIndex(column), visualIndex); } void LinkTreeWidget::setColumnDataFunction(int column, ColumnDataFunction func) { impl->columnInfos[column].dataFunction = func; } void LinkTreeWidget::setColumnSetDataFunction(int column, ColumnSetDataFunction func) { impl->columnInfos[column].setDataFunction = func; } void LinkTreeWidget::setColumnWidgetFunction(int column, ColumnWidgetFunction func) { impl->columnInfos[column].widgetFunction = func; } void LinkTreeWidget::changeEvent(QEvent* event) { QTreeWidget::changeEvent(event); if(event->type() == QEvent::StyleChange){ //impl->setCurrentBodyItem(impl->currentBodyItem, true); } } void LinkTreeWidget::setAlignedItemWidget (LinkTreeItem* item, int column, QWidget* widget, Qt::Alignment alignment) { QHBoxLayout* box = new QHBoxLayout(); box->setContentsMargins(0, 0, 0, 0); if(impl->itemWidgetWidthAdjustment > 0){ widget->setMinimumWidth(widget->sizeHint().width() + impl->itemWidgetWidthAdjustment); } box->addWidget(widget, 0, alignment); QWidget* base = new QWidget(); base->setLayout(box); setItemWidget(item, column, base); } QWidget* LinkTreeWidget::alignedItemWidget(LinkTreeItem* item, int column) { QWidget* base = itemWidget(item, column); if(base){ QLayout* layout = base->layout(); if(layout){ QLayoutItem* layoutItem = layout->itemAt(0); if(layoutItem){ QWidget* widget = layoutItem->widget(); if(widget){ return widget; } } } } return 0; } void LinkTreeWidget::addCustomRow(LinkTreeItem* treeItem) { impl->customRows.push_back(treeItem); } int LinkTreeWidget::numLinkTreeItems() { return impl->rowIndexCounter; } SignalProxy< boost::signal > LinkTreeWidget::sigUpdateRequest() { return impl->sigUpdateRequest; } ComboBox* LinkTreeWidget::listingModeCombo() { return &impl->listingModeCombo; } void LinkTreeWidget::setListingMode(ListingMode mode) { impl->listingMode = mode; impl->listingModeCombo.setCurrentIndex(mode); } void LinkTreeWidget::fixListingMode(bool on) { impl->listingModeCombo.setEnabled(on); } LinkTreeWidgetImpl::BodyItemInfoPtr LinkTreeWidgetImpl::getBodyItemInfo(BodyItemPtr bodyItem) { BodyItemInfoPtr info; if(bodyItem){ if(bodyItem == currentBodyItem){ info = currentBodyItemInfo; } else if(isCacheEnabled){ BodyItemInfoMap::iterator p = bodyItemInfoCache.find(bodyItem); if(p != bodyItemInfoCache.end()){ info = p->second; } } if(!info && bodyItem->findRootItem()){ if(!isCacheEnabled){ bodyItemInfoCache.clear(); } info = new BodyItemInfo(); info->detachedFromRootConnection = bodyItem->sigDetachedFromRoot().connect( bind(&LinkTreeWidgetImpl::onBodyItemDetachedFromRoot, this, bodyItem.get())); bodyItemInfoCache[bodyItem] = info; } if(info){ info->setNumLinks(bodyItem->body()->numLinks(), false); } } return info; } void LinkTreeWidgetImpl::onBodyItemDetachedFromRoot(BodyItem* bodyItem) { if(currentBodyItem.get() == bodyItem){ setCurrentBodyItem(0, false); } BodyItemInfoMap::iterator p = bodyItemInfoCache.find(bodyItem); if(p != bodyItemInfoCache.end()){ p->second->detachedFromRootConnection.disconnect(); bodyItemInfoCache.erase(p); } } void LinkTreeWidget::setBodyItem(BodyItemPtr bodyItem) { impl->setCurrentBodyItem(bodyItem, false); } BodyItem* LinkTreeWidget::bodyItem() { return impl->currentBodyItem.get(); } LinkTreeItem* LinkTreeWidget::itemOfLink(int linkIndex) { if(linkIndex < (int)impl->linkIndexToItemMap.size()){ return impl->linkIndexToItemMap[linkIndex]; } return 0; } void LinkTreeWidgetImpl::clearTreeItems() { // Take custom row items before calling clear() to prevent the items from being deleted. for(size_t i=0; i < customRows.size(); ++i){ LinkTreeItem* item = customRows[i]; if(item->treeWidget()){ self->takeTopLevelItem(self->indexOfTopLevelItem(item)); } } self->clear(); } void LinkTreeWidgetImpl::setCurrentBodyItem(BodyItemPtr bodyItem, bool forceTreeUpdate) { if(TRACE_FUNCTIONS){ cout << "LinkTreeWidgetImpl::setCurrentBodyItem()" << endl; } if(forceTreeUpdate || bodyItem != currentBodyItem){ self->blockSignals(true); clearTreeItems(); rowIndexCounter = 0; linkIndexToItemMap.clear(); if(QApplication::style()->objectName() == "cleanlooks"){ itemWidgetWidthAdjustment = 2; } else { itemWidgetWidthAdjustment = 0; } self->blockSignals(false); currentBodyItemInfo = getBodyItemInfo(bodyItem); currentBodyItem = bodyItem; if(bodyItem){ BodyPtr body = bodyItem->body(); linkIndexToItemMap.resize(body->numLinks(), 0); switch(listingMode){ case LinkTreeWidget::LINK_LIST: self->setRootIsDecorated(false); setLinkList(body); break; case LinkTreeWidget::LINK_TREE: self->setRootIsDecorated(true); setLinkTree(body->rootLink(), false); break; case LinkTreeWidget::JOINT_LIST: self->setRootIsDecorated(false); setJointList(body); break; case LinkTreeWidget::JOINT_TREE: self->setRootIsDecorated(true); setLinkTree(body->rootLink(), true); break; case LinkTreeWidget::PART_TREE: self->setRootIsDecorated(true); setLinkGroupTree(bodyItem); break; default: break; } addCustomRows(); sigUpdateRequest(true); } } } void LinkTreeWidgetImpl::restoreTreeState() { if(TRACE_FUNCTIONS){ cout << "LinkTreeWidgetImpl::restoreTreeState()" << endl; } if(currentBodyItemInfo){ restoreSubTreeState(self->invisibleRootItem()); currentBodyItemInfo->isRestoringTreeStateNeeded = false; } } void LinkTreeWidgetImpl::restoreSubTreeState(QTreeWidgetItem* item) { self->blockSignals(true); restoreTreeStateSub(item); self->blockSignals(false); } void LinkTreeWidgetImpl::restoreTreeStateSub(QTreeWidgetItem* parentItem) { if(TRACE_FUNCTIONS){ cout << "LinkTreeWidgetImpl::restoreTreeStateSub()" << endl; } int n = parentItem->childCount(); for(int i=0; i < n; ++i){ LinkTreeItem* item = dynamic_cast(parentItem->child(i)); if(item){ const Link* link = item->link(); if(link){ const dynamic_bitset<>& selection = currentBodyItemInfo->selection; item->setSelected(selection[link->index]); } if(item->childCount() > 0){ // Tree bool expanded = item->isExpanded(); if(listingMode == LinkTreeWidget::PART_TREE){ if(!link){ const std::set& parts = currentBodyItemInfo->expandedParts; expanded = (parts.find(item->name()) != parts.end()); item->setExpanded(expanded); } } else if(link){ expanded = currentBodyItemInfo->linkExpansions[link->index]; item->setExpanded(expanded); } if(expanded){ restoreTreeStateSub(item); } } } } } void LinkTreeWidgetImpl::addChild(LinkTreeItem* parentItem, LinkTreeItem* item) { if(parentItem){ parentItem->addChild(item); } else { self->addTopLevelItem(item); } item->rowIndex_ = rowIndexCounter++; for(size_t col=0; col < columnInfos.size(); ++col){ LinkTreeWidget::ColumnWidgetFunction& func = columnInfos[col].widgetFunction; if(!func.empty()){ QWidget* widget = func(item); if(widget){ self->setItemWidget(item, col, widget); } } } } void LinkTreeWidgetImpl::addChild(LinkTreeItem* item) { addChild(0, item); } void LinkTreeWidgetImpl::setLinkTree(Link* link, bool onlyJoints) { self->blockSignals(true); setLinkTreeSub(link, 0, 0, onlyJoints); self->blockSignals(false); } void LinkTreeWidgetImpl::setLinkTreeSub(Link* link, Link* parentLink, LinkTreeItem* parentItem, bool onlyJoints) { LinkTreeItem* item = 0; if(onlyJoints && link->jointId < 0){ item = parentItem; } else { item = new LinkTreeItem(link, this); addChild(parentItem, item); item->setExpanded(true); } if(link->child){ for(Link* child = link->child; child; child = child->sibling){ setLinkTreeSub(child, link, item, onlyJoints); } } } void LinkTreeWidgetImpl::setJointList(const BodyPtr& body) { for(int i=0; i < body->numJoints(); ++i){ Link* joint = body->joint(i); if(joint->isValid()){ addChild(new LinkTreeItem(joint, this)); } } } void LinkTreeWidgetImpl::setLinkList(const BodyPtr& body) { if(TRACE_FUNCTIONS){ cout << "LinkTreeWidgetImpl::setLinkList(" << body->name() << ")" << endl; } for(int i=0; i < body->numLinks(); ++i){ addChild(new LinkTreeItem(body->link(i), this)); } } void LinkTreeWidgetImpl::setLinkGroupTree(const BodyItemPtr bodyItem) { BodyPtr body = bodyItem->body(); LinkGroupPtr linkGroup = body->linkGroup(); if(linkGroup){ self->blockSignals(true); setLinkGroupTreeSub(0, linkGroup, body); self->blockSignals(false); } } void LinkTreeWidgetImpl::setLinkGroupTreeSub(LinkTreeItem* parentItem, const LinkGroupPtr& linkGroup, const BodyPtr& body) { LinkTreeItem* item = new LinkTreeItem(linkGroup.get(), this); addChild(parentItem, item); int numChildGroups = 0; int n = linkGroup->numElements(); for(int i=0; i < n; ++i){ if(linkGroup->isSubGroup(i)){ setLinkGroupTreeSub(item, linkGroup->subGroup(i), body); ++numChildGroups; } else if(linkGroup->isLinkIndex(i)){ Link* link = body->link(linkGroup->linkIndex(i)); if(link){ addChild(item, new LinkTreeItem(link, this)); } } } item->setExpanded(numChildGroups > 0); } void LinkTreeWidgetImpl::addCustomRows() { for(size_t i=0; i < customRows.size(); ++i){ addChild(customRows[i]); } } void LinkTreeWidgetImpl::onListingModeChanged(int index) { LinkTreeWidget::ListingMode newListingMode = static_cast(index); if(newListingMode != listingMode){ listingMode = newListingMode; if(currentBodyItem){ setCurrentBodyItem(currentBodyItem, true); } } } void LinkTreeWidget::onSelectionChanged() { impl->onSelectionChanged(); } void LinkTreeWidgetImpl::onSelectionChanged() { if(TRACE_FUNCTIONS){ cout << "LinkTreeWidgetImpl::onSelectionChanged()" << endl; } if(currentBodyItem){ currentBodyItemInfo->selection.reset(); QList selected = self->selectedItems(); for(int i=0; i < selected.size(); ++i){ LinkTreeItem* item = dynamic_cast(selected[i]); if(item && item->link()){ currentBodyItemInfo->selection[item->link()->index] = true; } } currentBodyItemInfo->sigSelectionChanged(); sigSelectionChanged(); } } SignalProxy< boost::signal > LinkTreeWidget::sigItemChanged() { return impl->sigItemChanged; } void LinkTreeWidget::onItemChanged(QTreeWidgetItem* item, int column) { LinkTreeItem* linkTreeItem = dynamic_cast(item); if(linkTreeItem){ impl->sigItemChanged(linkTreeItem, column); } } SignalProxy< boost::signal > LinkTreeWidget::sigSelectionChanged() { return impl->sigSelectionChanged; } SignalProxy< boost::signal > LinkTreeWidget::sigSelectionChanged(BodyItemPtr bodyItem) { return impl->sigSelectionChangedOf(bodyItem); } boost::signal& LinkTreeWidgetImpl::sigSelectionChangedOf(BodyItemPtr bodyItem) { BodyItemInfoPtr info = getBodyItemInfo(bodyItem); if(info){ return info->sigSelectionChanged; } return dummySigSelectionChanged; } const std::vector& LinkTreeWidget::getSelectedLinkIndices() { return impl->getSelectedLinkIndices(impl->currentBodyItem); } const std::vector& LinkTreeWidget::getSelectedLinkIndices(BodyItemPtr bodyItem) { return impl->getSelectedLinkIndices(bodyItem); } const std::vector& LinkTreeWidgetImpl::getSelectedLinkIndices(BodyItemPtr bodyItem) { BodyItemInfoPtr info = getBodyItemInfo(bodyItem); if(info){ info->selectedLinkIndices.clear(); const dynamic_bitset<>& selection = info->selection; for(size_t i=0; i < selection.size(); ++i){ if(selection[i]){ info->selectedLinkIndices.push_back(i); } } return info->selectedLinkIndices; } return emptyLinkIndices; } const boost::dynamic_bitset<>& LinkTreeWidget::getLinkSelection() { return impl->getLinkSelection(impl->currentBodyItem); } const boost::dynamic_bitset<>& LinkTreeWidget::getLinkSelection(BodyItemPtr bodyItem) { return impl->getLinkSelection(bodyItem); } const boost::dynamic_bitset<>& LinkTreeWidgetImpl::getLinkSelection(BodyItemPtr bodyItem) { BodyItemInfoPtr info = getBodyItemInfo(bodyItem); if(info){ return info->selection; } return emptySelection; } void LinkTreeWidget::onCustomContextMenuRequested(const QPoint& pos) { impl->onCustomContextMenuRequested(pos); } void LinkTreeWidgetImpl::onCustomContextMenuRequested(const QPoint& pos) { popupMenu.popup(pos); } void LinkTreeWidgetImpl::setExpansionState(const LinkTreeItem* item, bool on) { if(listingMode == LinkTreeWidget::LINK_TREE || listingMode == LinkTreeWidget::JOINT_TREE){ if(item->link()){ currentBodyItemInfo->linkExpansions[item->link()->index] = on; } } else if(listingMode == LinkTreeWidget::PART_TREE){ if(on){ currentBodyItemInfo->expandedParts.insert(item->name()); } else { currentBodyItemInfo->expandedParts.erase(item->name()); } } } void LinkTreeWidget::onItemExpanded(QTreeWidgetItem* treeWidgetItem) { impl->onItemExpanded(treeWidgetItem); } void LinkTreeWidgetImpl::onItemExpanded(QTreeWidgetItem* treeWidgetItem) { if(TRACE_FUNCTIONS){ cout << "LinkTreeWidgetImpl::onItemExpanded()" << endl; } LinkTreeItem* item = dynamic_cast(treeWidgetItem); if(item){ setExpansionState(item, true); restoreSubTreeState(item); } } void LinkTreeWidget::onItemCollapsed(QTreeWidgetItem* treeWidgetItem) { impl->onItemCollapsed(treeWidgetItem); } void LinkTreeWidgetImpl::onItemCollapsed(QTreeWidgetItem* treeWidgetItem) { if(TRACE_FUNCTIONS){ cout << "LinkTreeWidgetImpl::onItemCollapsed()" << endl; } LinkTreeItem* item = dynamic_cast(treeWidgetItem); if(item){ setExpansionState(item, false); } } void LinkTreeWidget::onHeaderSectionResized() { updateGeometry(); } void LinkTreeWidget::enableArchiveOfCurrentBodyItem(bool on) { impl->isArchiveOfCurrentBodyItemEnabled = on; } bool LinkTreeWidget::makeSingleSelection(BodyItemPtr bodyItem, int linkIndex) { return impl->makeSingleSelection(bodyItem, linkIndex); } bool LinkTreeWidgetImpl::makeSingleSelection(BodyItemPtr& bodyItem, int linkIndex) { BodyItemInfoPtr info = getBodyItemInfo(bodyItem); if(!info){ return false; } if(static_cast(linkIndex) < info->selection.size()){ if(!info->selection[linkIndex] || info->selection.count() > 1){ info->selection.reset(); info->selection.set(linkIndex); if(bodyItem == currentBodyItem){ restoreTreeState(); LinkTreeItem* item = linkIndexToItemMap[linkIndex]; if(item){ self->scrollToItem(item); } currentBodyItemInfo->sigSelectionChanged(); sigSelectionChanged(); } else { info->sigSelectionChanged(); } } } return true; } MenuManager& LinkTreeWidget::popupMenuManager() { return impl->popupMenuManager; } bool LinkTreeWidget::storeState(Archive& archive) { return impl->storeState(archive); } bool LinkTreeWidgetImpl::storeState(Archive& archive) { if(listingModeCombo.isEnabled()){ archive.write("listingMode", listingModeCombo.currentOrgText().toStdString(), YAML_DOUBLE_QUOTED); } if(isArchiveOfCurrentBodyItemEnabled){ archive.writeItemId("currentBodyItem", currentBodyItem); } if(isCacheEnabled && !bodyItemInfoCache.empty()){ YamlSequencePtr bodyItemNodes = new YamlSequence(); for(BodyItemInfoMap::iterator it = bodyItemInfoCache.begin(); it != bodyItemInfoCache.end(); ++it){ BodyItemPtr bodyItem = it->first; BodyItemInfo& info = *it->second; YamlMappingPtr bodyItemNode = new YamlMapping(); bool isEmpty = true; bodyItemNode->write("id", archive.getItemId(bodyItem)); const vector& indices = getSelectedLinkIndices(bodyItem); if(!indices.empty()){ YamlSequence& selected = *bodyItemNode->createFlowStyleSequence("selectedLinks"); int n = indices.size(); for(int i=0; i < n; ++i){ selected.append(indices[i], 20, n); } isEmpty = false; } int n = info.linkExpansions.size(); int m = n - info.linkExpansions.count(); if(m > 0){ YamlSequence& nonExpanded = *bodyItemNode->createFlowStyleSequence("nonExpandedLinks"); for(int i=0; i < n; ++i){ if(!info.linkExpansions[i]){ nonExpanded.append(i, 20, m); } } isEmpty = false; } n = info.expandedParts.size(); if(n > 0){ YamlSequence& expanded = *bodyItemNode->createFlowStyleSequence("expandedParts"); for(std::set::iterator p = info.expandedParts.begin(); p != info.expandedParts.end(); ++p){ expanded.append(*p, 10, n, YAML_DOUBLE_QUOTED); } isEmpty = false; } if(!isEmpty){ bodyItemNodes->append(bodyItemNode); } } if(!bodyItemNodes->empty()){ archive.insert("bodyItems", bodyItemNodes); } } return true; } bool LinkTreeWidget::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool LinkTreeWidgetImpl::restoreState(const Archive& archive) { if(isCacheEnabled){ const YamlSequence& nodes = *archive.findSequence("bodyItems"); if(nodes.isValid() && !nodes.empty()){ for(int i=0; i < nodes.size(); ++i){ const YamlMapping& node = *nodes[i].toMapping(); int id = node.get("id", -1); if(id >= 0){ BodyItem* bodyItem = archive.findItem(id); if(bodyItem){ int numLinks = bodyItem->body()->numLinks(); BodyItemInfoPtr info = getBodyItemInfo(bodyItem); const YamlSequence& selected = *node.findSequence("selectedLinks"); if(selected.isValid()){ info->setNumLinks(numLinks, true); for(int i=0; i < selected.size(); ++i){ int index = selected[i].toInt(); if(index < numLinks){ info->selection[index] = true; } } } const YamlSequence& expanded = *node.findSequence("expandedParts"); for(int i=0; i < expanded.size(); ++i){ info->expandedParts.insert(expanded[i]); } } } } } } bool updateListingMode = false; string listingModeString; if(archive.read("listingMode", listingModeString)){ if(listingModeString != listingModeCombo.currentOrgText().toStdString()){ listingModeCombo.blockSignals(true); if(listingModeCombo.findOrgText(listingModeString, true) >= 0){ updateListingMode = true; } listingModeCombo.blockSignals(false); } } if(isArchiveOfCurrentBodyItemEnabled){ setCurrentBodyItem(archive.findItem("currentBodyItem"), updateListingMode); } else if(updateListingMode){ setCurrentBodyItem(currentBodyItem, true); } else { restoreTreeState(); } return true; } choreonoid-1.1.0+dfsg/src/BodyPlugin/LinkTreeWidget.h000066400000000000000000000111551207742442300224660ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_LINK_TREE_WIDGET_H_INCLUDED #define CNOID_BODYPLUGIN_LINK_TREE_WIDGET_H_INCLUDED #include "BodyItem.h" #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class MenuManager; class LinkGroup; class LinkTreeWidget; class LinkTreeWidgetImpl; class CNOID_EXPORT LinkTreeItem : public QTreeWidgetItem { public: LinkTreeItem(const std::string& name); inline int rowIndex() const { return rowIndex_; } inline const std::string& name() const { return name_; } inline const Link* link() const { return link_; } inline Link* link() { return link_; } inline bool isLinkGroup() const { return isLinkGroup_; } virtual QVariant data(int column, int role) const; virtual void setData(int column, int role, const QVariant& value); private: int rowIndex_; std::string name_; Link* link_; bool isLinkGroup_; LinkTreeItem(Link* link, LinkTreeWidgetImpl* treeImpl); LinkTreeItem(LinkGroup* linkGroup, LinkTreeWidgetImpl* treeImpl); friend class LinkTreeWidget; friend class LinkTreeWidgetImpl; }; class CNOID_EXPORT LinkTreeWidget : public TreeWidget { Q_OBJECT public: LinkTreeWidget(QWidget* parent = 0); virtual ~LinkTreeWidget(); void setDefaultExpansionLevel(int level); void enableCache(bool on); enum ListingMode { LINK_LIST, LINK_TREE, JOINT_LIST, JOINT_TREE, PART_TREE }; ComboBox* listingModeCombo(); void setListingMode(ListingMode mode); void fixListingMode(bool on = true); SignalProxy< boost::signal > sigUpdateRequest(); void setBodyItem(BodyItemPtr bodyItem); BodyItem* bodyItem(); typedef boost::function ColumnDataFunction; typedef boost::function ColumnSetDataFunction; typedef boost::function ColumnWidgetFunction; int addColumn(); int addColumn(const QString& headerText); void setColumnDataFunction(int column, ColumnDataFunction func); void setColumnSetDataFunction(int column, ColumnSetDataFunction func); void setColumnWidgetFunction(int column, ColumnWidgetFunction func); void moveVisualColumnIndex(int column, int visualIndex); int nameColumn(); int jointIdColumn(); void setAlignedItemWidget(LinkTreeItem* item, int column, QWidget* widget, Qt::Alignment alignment = Qt::AlignCenter); QWidget* alignedItemWidget(LinkTreeItem* item, int column); void addCustomRow(LinkTreeItem* treeItem); LinkTreeItem* itemOfLink(int linkIndex); int numLinkTreeItems(); SignalProxy< boost::signal > sigItemChanged(); SignalProxy< boost::signal > sigSelectionChanged(); const std::vector& getSelectedLinkIndices(); const boost::dynamic_bitset<>& getLinkSelection(); /// This signal is available after calling 'enableCache(true)'. SignalProxy< boost::signal > sigSelectionChanged(BodyItemPtr bodyItem); /// This member is available after calling 'enableCache(true)'. const std::vector& getSelectedLinkIndices(BodyItemPtr bodyItem); /// This member is available after calling 'enableCache(true)'. const boost::dynamic_bitset<>& getLinkSelection(BodyItemPtr bodyItem); MenuManager& popupMenuManager(); bool makeSingleSelection(BodyItemPtr bodyItem, int linkIndex); void enableArchiveOfCurrentBodyItem(bool on); bool storeState(Archive& archive); bool restoreState(const Archive& archive); protected: virtual void changeEvent(QEvent* event); private Q_SLOTS: void onItemChanged(QTreeWidgetItem* item, int column); void onSelectionChanged(); void onCustomContextMenuRequested(const QPoint& pos); void onItemExpanded(QTreeWidgetItem* treeWidgetItem); void onItemCollapsed(QTreeWidgetItem* treeWidgetItem); void onHeaderSectionResized(); private: LinkTreeWidgetImpl* impl; friend class LinkTreeItem; }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/MultiAffine3SeqGraphView.cpp000066400000000000000000000232551207742442300247200ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "MultiAffine3SeqGraphView.h" #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } MultiAffine3SeqGraphView::MultiAffine3SeqGraphView() : graph(this) { setName(N_("Multi Affine3 Seq")); setDefaultLayoutArea(View::BOTTOM); static const char* xyzLabels[] = { "X", "Y", "Z" }; static const char* rpyLabels[] = { "R", "P", "Y" }; QHBoxLayout* hbox = new QHBoxLayout(); hbox->setSpacing(0); QVBoxLayout* vbox = new QVBoxLayout(); vbox->setSpacing(0); vbox->addStretch(); setupElementToggleSet(vbox, xyzToggles, xyzLabels, true); setupElementToggleSet(vbox, rpyToggles, rpyLabels, false); vbox->addStretch(); hbox->addLayout(vbox); hbox->addWidget(&graph, 1); setLayout(hbox); ItemTreeView::mainInstance()->sigSelectionChanged().connect( bind(&MultiAffine3SeqGraphView::onItemSelectionChanged, this, _1)); linkSelection = LinkSelectionView::mainInstance(); } void MultiAffine3SeqGraphView::setupElementToggleSet (QBoxLayout* box, ToggleToolButton toggles[], const char* labels[], bool isActive) { for(int i=0; i < 3; ++i){ toggles[i].setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); box->addWidget(&toggles[i]); toggles[i].setChecked(isActive); toggles[i].setText(labels[i]); toggleConnections.add( toggles[i].sigToggled().connect( bind(&MultiAffine3SeqGraphView::setupGraphWidget, this))); } } MultiAffine3SeqGraphView::~MultiAffine3SeqGraphView() { bodyItemConnections.disconnect(); } QWidget* MultiAffine3SeqGraphView::indicatorOnInfoBar() { return &graph.statusLabel(); } void MultiAffine3SeqGraphView::onItemSelectionChanged(const ItemList& items) { if(items.empty()){ return; } if(itemInfos.size() == items.size()){ bool unchanged = true; int i=0; for(list::iterator it = itemInfos.begin(); it != itemInfos.end(); ++it){ if(it->item != items[i++]){ unchanged = false; break; } } if(unchanged){ return; } } itemInfos.clear(); for(size_t i=0; i < items.size(); ++i){ BodyItemPtr bodyItem = items[i]->findOwnerItem(); if(bodyItem){ itemInfos.push_back(ItemInfo()); list::iterator it = --itemInfos.end(); it->item = items[i]; it->seq = it->item->seq(); it->bodyItem = bodyItem; it->connections.add(it->item->sigUpdated().connect( bind(&MultiAffine3SeqGraphView::onDataItemUpdated, this, it))); it->connections.add(it->item->sigDetachedFromRoot().connect( bind(&MultiAffine3SeqGraphView::onDataItemDetachedFromRoot, this, it))); } } updateBodyItems(); setupGraphWidget(); } void MultiAffine3SeqGraphView::onDataItemDetachedFromRoot(std::list::iterator itemInfoIter) { itemInfos.erase(itemInfoIter); updateBodyItems(); setupGraphWidget(); } void MultiAffine3SeqGraphView::updateBodyItems() { bodyItemConnections.disconnect(); bodyItems.clear(); for(list::iterator it = itemInfos.begin(); it != itemInfos.end(); ++it){ set::iterator p = bodyItems.find(it->bodyItem); if(p == bodyItems.end()){ bodyItems.insert(it->bodyItem); bodyItemConnections.add( linkSelection->sigSelectionChanged(it->bodyItem).connect( bind(&MultiAffine3SeqGraphView::setupGraphWidget, this))); bodyItemConnections.add( it->bodyItem->sigDetachedFromRoot().connect( bind(&MultiAffine3SeqGraphView::onBodyItemDetachedFromRoot, this, it->bodyItem))); } } } void MultiAffine3SeqGraphView::onBodyItemDetachedFromRoot(BodyItemPtr bodyItem) { bool erased = false; list::iterator it = itemInfos.begin(); while(it != itemInfos.end()){ if(it->bodyItem == bodyItem){ it = itemInfos.erase(it); erased = true; } else { ++it; } } if(erased){ updateBodyItems(); setupGraphWidget(); } } void MultiAffine3SeqGraphView::setupGraphWidget() { graph.clearDataHandlers(); for(list::iterator it = itemInfos.begin(); it != itemInfos.end(); ++it){ if(it->bodyItem){ MultiAffine3SeqPtr seq = it->item->seq(); int numParts = seq->numParts(); BodyPtr body = it->bodyItem->body(); const std::vector& selectedLinkIndices = linkSelection->getSelectedLinkIndices(it->bodyItem); for(size_t i=0; i < selectedLinkIndices.size(); ++i){ Link* link = body->link(selectedLinkIndices[i]); if(link && link->index < numParts){ addPositionTrajectory(it, link, seq); } } } } } void MultiAffine3SeqGraphView::addPositionTrajectory (std::list::iterator itemInfoIter, Link* link, MultiAffine3SeqPtr seq) { for(int i=0; i < 2; ++i){ ToggleToolButton* toggles = (i == 0) ? xyzToggles : rpyToggles; for(int j=0; j < 3; ++j){ if(toggles[j].isChecked()){ GraphDataHandlerPtr handler(new GraphDataHandler()); handler->setLabel(link->name()); handler->setFrameProperties(seq->numFrames(), seq->frameRate()); handler->setDataRequestCallback( bind(&MultiAffine3SeqGraphView::onDataRequest, this, itemInfoIter, link->index, i, j, _1, _2, _3)); handler->setDataModifiedCallback( bind(&MultiAffine3SeqGraphView::onDataModified, this, itemInfoIter, link->index, i, j, _1, _2, _3)); graph.addDataHandler(handler); itemInfoIter->handlers.push_back(handler); } } } } void MultiAffine3SeqGraphView::onDataItemUpdated(std::list::iterator itemInfoIter) { if(TRACE_FUNCTIONS){ cout << "MultiAffine3SeqGraphView::onDataItemUpdated()" << endl; } const MultiAffine3SeqPtr& seq = itemInfoIter->item->seq(); int newNumFrames = seq->numFrames(); double newFrameRate = seq->frameRate(); for(size_t i=0; i < itemInfoIter->handlers.size(); ++i){ if(TRACE_FUNCTIONS){ cout << "MultiAffine3SeqGraphView::onDataItemUpdated(" << i << ")" << endl; } itemInfoIter->handlers[i]->setFrameProperties(newNumFrames, newFrameRate); itemInfoIter->handlers[i]->update(); } } void MultiAffine3SeqGraphView::onDataRequest (std::list::iterator itemInfoIter, int linkIndex, int type, int axis, int frame, int size, double* out_values) { MultiAffine3Seq::View part = itemInfoIter->seq->part(linkIndex); if(type == 0){ // xyz for(int i=0; i < size; ++i){ const Affine3& p = part[frame + i]; out_values[i] = p.translation()[axis]; } } else { // rpy for(int i=0; i < size; ++i){ const Affine3& p = part[frame + i]; out_values[i] = rpyFromRot(p.linear())[axis]; } } } void MultiAffine3SeqGraphView::onDataModified (std::list::iterator itemInfoIter, int linkIndex, int type, int axis, int frame, int size, double* values) { MultiAffine3Seq::View part = itemInfoIter->seq->part(linkIndex); if(type == 0){ // xyz for(int i=0; i < size; ++i){ Affine3& p = part[frame + i]; p.translation()[axis] = values[i]; } } else { // rpy for(int i=0; i < size; ++i){ Affine3& p = part[frame + i]; Vector3 rpy(rpyFromRot(p.linear())); rpy[axis] = values[i]; p.linear() = rotFromRpy(rpy); } } itemInfoIter->connections.block(); itemInfoIter->item->notifyUpdate(); itemInfoIter->connections.unblock(); } bool MultiAffine3SeqGraphView::storeState(Archive& archive) { if(graph.storeState(archive)){ YamlSequence& visibleElements = *archive.createFlowStyleSequence("visibleElements"); for(int i=0; i < 3; ++i){ if(xyzToggles[i].isChecked()){ visibleElements.append(i); } } for(int i=0; i < 3; ++i){ if(rpyToggles[i].isChecked()){ visibleElements.append(i+3); } } return true; } return false; } bool MultiAffine3SeqGraphView::restoreState(const Archive& archive) { if(graph.restoreState(archive)){ const YamlSequence& visibleElements = *archive.findSequence("visibleElements"); if(visibleElements.isValid()){ toggleConnections.block(); for(int i=0; i < 3; ++i){ xyzToggles[i].setChecked(false); rpyToggles[i].setChecked(false); } for(int i=0; i < visibleElements.size(); ++i){ int index = visibleElements[i].toInt(); if(index < 3){ xyzToggles[index].setChecked(true); } else { rpyToggles[index-3].setChecked(true); } } toggleConnections.unblock(); } return true; } return false; } choreonoid-1.1.0+dfsg/src/BodyPlugin/MultiAffine3SeqGraphView.h000066400000000000000000000053051207742442300243610ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_MULTI_AFFINE_SEQ_GRAPH_VIEW_H_INCLUDED #define CNOID_BODYPLUGIN_MULTI_AFFINE_SEQ_GRAPH_VIEW_H_INCLUDED #include "BodyItem.h" #include "LinkSelectionView.h" #include #include #include #include #include #include #include #include #include #include #include #include namespace cnoid { class Archive; /** @todo Define and implement the API for installing an index selection interface and move this class into GuiBase module */ class MultiAffine3SeqGraphView : public View, public boost::signals::trackable { public: MultiAffine3SeqGraphView(); ~MultiAffine3SeqGraphView(); virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); protected: virtual QWidget* indicatorOnInfoBar(); private: GraphWidget graph; LinkSelectionView* linkSelection; ToggleToolButton xyzToggles[3]; ToggleToolButton rpyToggles[3]; ConnectionSet toggleConnections; struct ItemInfo { ~ItemInfo(){ connections.disconnect(); } MultiAffine3SeqItemPtr item; MultiAffine3SeqPtr seq; BodyItemPtr bodyItem; ConnectionSet connections; std::vector handlers; }; std::list itemInfos; std::set bodyItems; ConnectionSet bodyItemConnections; void setupElementToggleSet(QBoxLayout* box, ToggleToolButton toggles[], const char* labels[], bool isActive); void onItemSelectionChanged(const ItemList& dataItems); void onDataItemDetachedFromRoot(std::list::iterator itemInfoIter); void updateBodyItems(); void onBodyItemDetachedFromRoot(BodyItemPtr bodyItem); void setupGraphWidget(); void addPositionTrajectory(std::list::iterator itemInfoIter, Link* link, MultiAffine3SeqPtr seq); void onDataItemUpdated(std::list::iterator itemInfoIter); void onDataRequest( std::list::iterator itemInfoIter, int linkIndex, int type, int axis, int frame, int size, double* out_values); void onDataModified( std::list::iterator itemInfoIter, int linkIndex, int type, int axis, int frame, int size, double* values); }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/MultiValueSeqGraphView.cpp000066400000000000000000000153741207742442300245240ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "MultiValueSeqGraphView.h" #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } MultiValueSeqGraphView::MultiValueSeqGraphView() : graph(this) { setDefaultLayoutArea(View::BOTTOM); setName(N_("Multi Value Seq")); QVBoxLayout* vbox = new QVBoxLayout(); vbox->addWidget(&graph); setLayout(vbox); ItemTreeView::mainInstance()->sigSelectionChanged().connect( bind(&MultiValueSeqGraphView::onItemSelectionChanged, this, _1)); linkSelection = LinkSelectionView::mainInstance(); } MultiValueSeqGraphView::~MultiValueSeqGraphView() { bodyItemConnections.disconnect(); } QWidget* MultiValueSeqGraphView::indicatorOnInfoBar() { return &graph.statusLabel(); } void MultiValueSeqGraphView::onItemSelectionChanged(const ItemList& items) { if(items.empty()){ return; } if(itemInfos.size() == items.size()){ bool unchanged = true; int i=0; for(list::iterator it = itemInfos.begin(); it != itemInfos.end(); ++it){ if(it->item != items[i++]){ unchanged = false; break; } } if(unchanged){ return; } } itemInfos.clear(); for(size_t i=0; i < items.size(); ++i){ BodyItemPtr bodyItem = items[i]->findOwnerItem(); if(bodyItem){ itemInfos.push_back(ItemInfo()); list::iterator it = --itemInfos.end(); it->item = items[i]; it->seq = it->item->seq(); it->bodyItem = bodyItem; it->connections.add(it->item->sigUpdated().connect( bind(&MultiValueSeqGraphView::onDataItemUpdated, this, it))); it->connections.add(it->item->sigDetachedFromRoot().connect( bind(&MultiValueSeqGraphView::onDataItemDetachedFromRoot, this, it))); } } updateBodyItems(); setupGraphWidget(); } void MultiValueSeqGraphView::onDataItemDetachedFromRoot(std::list::iterator itemInfoIter) { itemInfos.erase(itemInfoIter); updateBodyItems(); setupGraphWidget(); } void MultiValueSeqGraphView::updateBodyItems() { bodyItemConnections.disconnect(); bodyItems.clear(); for(list::iterator it = itemInfos.begin(); it != itemInfos.end(); ++it){ set::iterator p = bodyItems.find(it->bodyItem); if(p == bodyItems.end()){ bodyItems.insert(it->bodyItem); bodyItemConnections.add( linkSelection->sigSelectionChanged(it->bodyItem).connect( bind(&MultiValueSeqGraphView::setupGraphWidget, this))); bodyItemConnections.add( it->bodyItem->sigDetachedFromRoot().connect( bind(&MultiValueSeqGraphView::onBodyItemDetachedFromRoot, this, it->bodyItem))); } } } void MultiValueSeqGraphView::onBodyItemDetachedFromRoot(BodyItemPtr bodyItem) { bool erased = false; list::iterator it = itemInfos.begin(); while(it != itemInfos.end()){ if(it->bodyItem == bodyItem){ it = itemInfos.erase(it); erased = true; } else { ++it; } } if(erased){ updateBodyItems(); setupGraphWidget(); } } void MultiValueSeqGraphView::setupGraphWidget() { graph.clearDataHandlers(); for(list::iterator it = itemInfos.begin(); it != itemInfos.end(); ++it){ if(it->bodyItem){ MultiValueSeqPtr seq = it->item->seq(); int numParts = seq->numParts(); BodyPtr body = it->bodyItem->body(); const std::vector& selectedLinkIndices = linkSelection->getSelectedLinkIndices(it->bodyItem); for(size_t i=0; i < selectedLinkIndices.size(); ++i){ Link* link = body->link(selectedLinkIndices[i]); if(link && link->jointId >= 0 && link->jointId < numParts){ addJointTrajectory(it, link, seq); } } } } } void MultiValueSeqGraphView::addJointTrajectory(std::list::iterator itemInfoIter, Link* joint, MultiValueSeqPtr seq) { GraphDataHandlerPtr handler(new GraphDataHandler()); handler->setLabel(joint->name()); handler->setValueLimits(joint->llimit, joint->ulimit); handler->setVelocityLimits(joint->lvlimit, joint->uvlimit); handler->setFrameProperties(seq->numFrames(), seq->frameRate()); handler->setDataRequestCallback( bind(&MultiValueSeqGraphView::onDataRequest, this, itemInfoIter, joint->jointId, _1, _2, _3)); handler->setDataModifiedCallback( bind(&MultiValueSeqGraphView::onDataModified, this, itemInfoIter, joint->jointId, _1, _2, _3)); graph.addDataHandler(handler); itemInfoIter->handlers.push_back(handler); } void MultiValueSeqGraphView::onDataItemUpdated(std::list::iterator itemInfoIter) { if(TRACE_FUNCTIONS){ cout << "MultiValueSeqGraphView::onDataItemUpdated()" << endl; } const MultiValueSeqPtr& seq = itemInfoIter->item->seq(); int newNumFrames = seq->numFrames(); double newFrameRate = seq->frameRate(); for(size_t i=0; i < itemInfoIter->handlers.size(); ++i){ if(TRACE_FUNCTIONS){ cout << "MultiValueSeqGraphView::onDataItemUpdated(" << i << ")" << endl; } itemInfoIter->handlers[i]->setFrameProperties(newNumFrames, newFrameRate); itemInfoIter->handlers[i]->update(); } } void MultiValueSeqGraphView::onDataRequest (std::list::iterator itemInfoIter, int jointId, int frame, int size, double* out_values) { MultiValueSeq::View part = itemInfoIter->seq->part(jointId); for(int i=0; i < size; ++i){ out_values[i] = part[frame + i]; } } void MultiValueSeqGraphView::onDataModified (std::list::iterator itemInfoIter, int jointId, int frame, int size, double* values) { MultiValueSeq::View part = itemInfoIter->seq->part(jointId); for(int i=0; i < size; ++i){ part[frame + i] = values[i]; } itemInfoIter->connections.block(); itemInfoIter->item->notifyUpdate(); itemInfoIter->connections.unblock(); } bool MultiValueSeqGraphView::storeState(Archive& archive) { return graph.storeState(archive); } bool MultiValueSeqGraphView::restoreState(const Archive& archive) { return graph.restoreState(archive); } choreonoid-1.1.0+dfsg/src/BodyPlugin/MultiValueSeqGraphView.h000066400000000000000000000044551207742442300241670ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_MULTI_VALUE_SEQ_GRAPH_VIEW_H_INCLUDED #define CNOID_BODYPLUGIN_MULTI_VALUE_SEQ_GRAPH_VIEW_H_INCLUDED #include "BodyItem.h" #include "LinkSelectionView.h" #include #include #include #include #include #include #include #include #include #include namespace cnoid { class Archive; /** @todo Define and implement the API for installing an index selection interface and move this class into GuiBase module */ class MultiValueSeqGraphView : public View, public boost::signals::trackable { public: MultiValueSeqGraphView(); ~MultiValueSeqGraphView(); virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); protected: virtual QWidget* indicatorOnInfoBar(); private: GraphWidget graph; LinkSelectionView* linkSelection; struct ItemInfo { ~ItemInfo(){ connections.disconnect(); } MultiValueSeqItemPtr item; MultiValueSeqPtr seq; BodyItemPtr bodyItem; ConnectionSet connections; std::vector handlers; }; std::list itemInfos; std::set bodyItems; ConnectionSet bodyItemConnections; void onItemSelectionChanged(const ItemList& dataItems); void onDataItemDetachedFromRoot(std::list::iterator itemInfoIter); void updateBodyItems(); void onBodyItemDetachedFromRoot(BodyItemPtr bodyItem); void setupGraphWidget(); void addJointTrajectory(std::list::iterator itemInfoIter, Link* joint, MultiValueSeqPtr seq); void onDataItemUpdated(std::list::iterator itemInfoIter); void onDataRequest(std::list::iterator itemInfoIter, int jointId, int frame, int size, double* out_values); void onDataModified(std::list::iterator itemInfoIter, int jointId, int frame, int size, double* values); }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/OsgCollision.cpp000066400000000000000000000032311207742442300225400ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "OsgCollision.h" #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } OsgCollision::OsgCollision() { ppairs = 0; setSupportsDisplayList(false); } OsgCollision::OsgCollision(const OsgCollision& org, const osg::CopyOp& copyop) : osg::Drawable(org, copyop) { ppairs = org.ppairs; setSupportsDisplayList(false); } /** \todo Use displaylists */ void OsgCollision::drawImplementation(osg::RenderInfo& ri) const { OsgViewer* viewer = dynamic_cast(ri.getView()); if(viewer && !viewer->isCollisionVisibleMode()){ return; } glPushAttrib(GL_ENABLE_BIT); glDisable(GL_LIGHTING); glColor3d(0.0, 1.0, 0.0); glBegin(GL_LINES); const std::vector& pairs = *ppairs; for(size_t i=0; i < pairs.size(); ++i){ const std::vector& cols = pairs[i]->collisions(); for(size_t j=0; j < cols.size(); ++j){ const collision_data& col = cols[j]; const Vector3 n = 50.0 * col.depth * col.n_vector; for(int k=0; k < col.num_of_i_points; ++k){ if(col.i_point_new[k]){ const Vector3& p = col.i_points[k]; glVertex3dv(p.data()); glVertex3dv(Vector3(p + n).data()); } } } } glEnd(); glPopAttrib(); } /** @todo improved the implementation. */ osg::BoundingBox OsgCollision::computeBound() const { osg::BoundingBox bbox; return bbox; } choreonoid-1.1.0+dfsg/src/BodyPlugin/OsgCollision.h000066400000000000000000000016041207742442300222070ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_OSG_COLLISION_H_INCLUDED #define CNOID_BODYPLUGIN_OSG_COLLISION_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { class OsgCollision : public osg::Drawable { public: OsgCollision(); OsgCollision(const OsgCollision& org, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY); META_Object(cnoidBody, OsgCollision) virtual ~OsgCollision() { } inline void setColdetPairs(const std::vector& pairs) { ppairs = &pairs; }; private: const std::vector* ppairs; virtual void drawImplementation(osg::RenderInfo& ri) const; virtual osg::BoundingBox computeBound() const; }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/SceneBody.cpp000066400000000000000000001250311207742442300220120ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "SceneBody.h" #include "OsgCollision.h" #include "KinematicsBar.h" #include "BodyBar.h" #include "LinkSelectionView.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; using osgGA::GUIEventAdapter; namespace { const bool TRACE_FUNCTIONS = false; #if OPENSCENEGRAPH_SOVERSION >= 55 typedef osg::Vec3d Vec3ForProjection; #else typedef osg::Vec3 Vec3ForProjection; #endif class SceneLink : public osg::MatrixTransform { public: SceneLink(); SceneLink(BodyPtr& body, JointNodeSetPtr jointNodeSet, VrmlToOsgConverter& vrmlToOsgConverter); void showMarker(const osg::Vec4 color, float width); void hideMarker(); Link* link; osg::ref_ptr shapeNode; osg::ref_ptr marker; osg::ref_ptr attitudeDragger; osg::ref_ptr outline; osg::ref_ptr scribe; bool isValid; bool isPointed; bool isColliding; }; } namespace cnoid { class SceneBodyImpl { public: SceneBodyImpl(SceneBody* self, BodyItemPtr bodyItem); ~SceneBodyImpl(); SceneBody* self; BodyItemPtr bodyItem; BodyPtr body; ConnectionSet connections; signals::connection connectionToSigWorldCollisionLinkSetChanged; std::vector< osg::ref_ptr > sceneLinks; osg::Group* visibleSceneLinks; osg::Group* markerGroup; SceneLink* outlinedLink; osg::ref_ptr osgCollision; osg::ref_ptr cmMarker; bool isCmVisible; osg::ref_ptr zmpMarker; bool isZmpVisible; Vector3 orgZmpPos; enum PointedType { PT_NONE, PT_SCENE_LINK, PT_ZMP }; SceneLink* pointedSceneLink; Link* targetLink; Vector3 orgPointerPos; Vector3 orgBaseLinkPos; Vector3 orgTargetLinkPos; double orgJointPosition; Vector3 rotationBaseX; Vector3 rotationBaseY; Matrix3 orgAttitude; Vector3 axis; JointPathPtr ikPath; LinkTraverse fkTraverse; PinDragIKptr pinDragIK; InverseKinematicsPtr ik; PenetrationBlockerPtr penetrationBlocker; osg::ref_ptr attitudeDragger; //vector snapFootLinkInfos; //bool isSnapping; //Matrix33 footAttitudeBeforeSnap; enum Axis { NOAXIS = -1, X = 0, Y = 1, Z = 2 }; Axis rotationAxis; enum DragMode { DRAG_NONE, LINK_IK_TRANSLATION, LINK_IK_ROTATION, LINK_FK_ROTATION, ZMP_TRANSLATION }; DragMode dragMode; bool isDragging; //osg::ref_ptr planeProjector; //osg::ref_ptr cylinderProjector; osg::ref_ptr projector; osgManipulator::PointerInfo pointerInfo; KinematicsBar* kinematicsBar; std::ostream& os; bool createSceneLinks(); void createSceneLinksSub(JointNodeSetPtr jointNodeSet, VrmlToOsgConverter& vrmlToOsgConverter); void onAttachedToScene(); void onDetachedFromScene(); void onKinematicStateChanged(); void createOutline(SceneLink* sceneLink); bool setPointedLinkVisual(SceneLink* sceneLink, bool on); bool setWorldCollisionLinkVisual(SceneLink* sceneLink, bool on); void createScribe(SceneLink* sceneLink); bool setSelfCollisionLinkVisual(SceneLink* sceneLink, bool on); void onSelfCollisionsUpdated(); void onWorldCollisionLinkSetChanged(); void onCollisionLinkSetChanged(); void onCollisionLinkHighlightModeChanged(); void changeCollisionLinkHighlightMode(bool on); void setLinkVisibilities(const boost::dynamic_bitset<>& visibilities); void showCenterOfMass(bool on); void showZmp(bool on); PointedType findPointedObject(const osg::NodePath& path); void setOutlineForPointedLink(SceneLink* sceneLink); void updateMarkersAndManipulators(); void makeLinkFree(SceneLink* sceneLink); void setBaseLink(SceneLink* sceneLink); void toggleBaseLink(SceneLink* sceneLink); void togglePin(SceneLink* sceneLink, bool toggleTranslation, bool toggleRotation); void showTargetLinkManipulators(bool on); bool onKeyPressEvent(const SceneViewEvent& event); bool onKeyReleaseEvent(const SceneViewEvent& event); bool onButtonPressEvent(const SceneViewEvent& event); bool onButtonReleaseEvent(const SceneViewEvent& event); bool onDoubleClickEvent(const SceneViewEvent& event); bool onPointerMoveEvent(const SceneViewEvent& event); void onPointerLeaveEvent(const SceneViewEvent& event); void onContextMenuRequest(const SceneViewEvent& event, MenuManager& menuManager); void onSceneModeChanged(); void makeLinkAttitudeLevel(); void setPlaneProjector(const SceneViewEvent& event); void startIK(const SceneViewEvent& event); //void setupFootSnap(); void attachAttitudeDraggerTo(Link* link); void dragIK(const SceneViewEvent& event); //void snapFoot(Vector3& p, Matrix33& R); void startFK(const SceneViewEvent& event); void dragFKRotation(const SceneViewEvent& event); void startZmpTranslation(const SceneViewEvent& event); void dragZmpTranslation(const SceneViewEvent& event); }; } SceneLink::SceneLink() { isValid = false; } SceneLink::SceneLink(BodyPtr& body, JointNodeSetPtr jointNodeSet, VrmlToOsgConverter& vrmlToOsgConverter) { isValid = true; isPointed = false; isColliding = false; const string& name = jointNodeSet->jointNode->defName; setName(name); link = body->link(name); vector& segmentNodes = jointNodeSet->segmentNodes; const JointNodeSet::Affine3Array& transforms = jointNodeSet->transforms; int numSegment = segmentNodes.size(); for(int i = 0 ; i < numSegment ; ++i){ shapeNode = vrmlToOsgConverter.convert(segmentNodes[i]); if(shapeNode.valid()){ if(link->Rs != Matrix3::Identity() || transforms[i].isApprox(Affine3::Identity())){ osg::MatrixTransform* transform = new osg::MatrixTransform(); Affine3 Rs; Rs.translation().setZero(); Rs.linear() = link->Rs; const Affine3 T(Rs * transforms[i]); const Matrix3& R = T.linear(); const Vector3& p = T.translation(); transform->setMatrix( osg::Matrix( R(0,0), R(1,0), R(2,0), 0, R(0,1), R(1,1), R(2,1), 0, R(0,2), R(1,2), R(2,2), 0, p(0), p(1), p(2), 1.0)); transform->addChild(shapeNode.get()); shapeNode = transform; } shapeNode->setDataVariance(osg::Object::STATIC); addChild(shapeNode.get()); /* VertexNormalVisualizer* normals = new VertexNormalVisualizer(shapeNode.get()); addChild(normals); */ } } } void SceneLink::showMarker(const osg::Vec4 color, float width) { if(!marker){ osg::ComputeBoundsVisitor cbv; shapeNode->accept(cbv); osg::BoundingBox& bbox = cbv.getBoundingBox(); //marker = new BBMarker(bbox, color, width); marker = new BBMarker(bbox, color); addChild(marker.get()); } } void SceneLink::hideMarker() { if(marker.valid()){ removeChild(marker.get()); marker = 0; } } SceneBody::SceneBody(BodyItemPtr bodyItem) { impl = new SceneBodyImpl(this, bodyItem); } SceneBodyImpl::SceneBodyImpl(SceneBody* self, BodyItemPtr bodyItem) : self(self), bodyItem(bodyItem), body(bodyItem->body()), kinematicsBar(KinematicsBar::instance()), os(MessageView::mainInstance()->cout()) { outlinedLink = 0; targetLink = 0; attitudeDragger = new AttitudeDragger(); osgCollision = new OsgCollision(); osgCollision->setColdetPairs(bodyItem->selfColdetPairs); osg::Geode* geode = new osg::Geode(); geode->addDrawable(osgCollision.get()); self->addChild(geode); markerGroup = new osg::Group(); self->addChild(markerGroup); cmMarker = new CrossMarker(osg::Vec4(0.0, 1.0, 0.0, 1.0), 0.25f, 2.0f); isCmVisible = false; zmpMarker = new SphereMarker(0.1, osg::Vec4(0.0f, 1.0f, 0.0f, 0.3f)); zmpMarker->setCross(osg::Vec4(0.0, 1.0, 0.0, 1.0), 0.25f, 2.0f); isZmpVisible = false; dragMode = DRAG_NONE; isDragging = false; self->setName(bodyItem->name()); self->setEditable(!body->isStaticModel()); visibleSceneLinks = new osg::Group(); self->addChild(visibleSceneLinks); kinematicsBar->sigCollisionVisualizationChanged().connect( bind(&SceneBodyImpl::onCollisionLinkHighlightModeChanged, this)); onCollisionLinkHighlightModeChanged(); } SceneBody::~SceneBody() { if(TRACE_FUNCTIONS){ cout << "SceneBody::~SceneBody()" << endl; } delete impl; } Link* SceneBody::getPointedSceneLink(){ if(impl->pointedSceneLink) return impl->pointedSceneLink->link; return 0; } osg::ref_ptr SceneBody::getPointedShapeNode(){ if(impl->pointedSceneLink) return impl->pointedSceneLink->shapeNode; return 0; } SceneBodyImpl::~SceneBodyImpl() { connectionToSigWorldCollisionLinkSetChanged.disconnect(); connections.disconnect(); } bool SceneBody::createSceneLinks() { return impl->createSceneLinks(); } bool SceneBodyImpl::createSceneLinks() { bool created = false; ModelNodeSetPtr modelNodeSet = bodyItem->modelNodeSet(); if(body && modelNodeSet){ VrmlToOsgConverter converter; sceneLinks.clear(); createSceneLinksSub(modelNodeSet->rootJointNodeSet(), converter); size_t numLinks = body->numLinks(); if(sceneLinks.size() < numLinks){ for(size_t i = sceneLinks.size(); i < numLinks; ++i){ sceneLinks.push_back(new SceneLink()); } } created = true; } return created; } void SceneBodyImpl::createSceneLinksSub(JointNodeSetPtr jointNodeSet, VrmlToOsgConverter& vrmlToOsgConverter) { SceneLink* sceneLink = new SceneLink(body, jointNodeSet, vrmlToOsgConverter); sceneLinks.push_back(sceneLink); visibleSceneLinks->addChild(sceneLink); vector& childJointNodeSets = jointNodeSet->childJointNodeSets; for(size_t i=0; i < childJointNodeSets.size(); ++i){ createSceneLinksSub(childJointNodeSets[i], vrmlToOsgConverter); } } void SceneBody::onAttachedToScene() { impl->onAttachedToScene(); } void SceneBodyImpl::onAttachedToScene() { if(sceneLinks.empty()){ createSceneLinks(); LeggedBody* legged = dynamic_cast(body.get()); if(legged && legged->numFeet() > 0){ Link* footLink = legged->footLink(0); SceneLink* sceneLink = sceneLinks[footLink->index]; osg::ComputeBoundsVisitor cbv; sceneLink->shapeNode->accept(cbv); osg::BoundingBox& bbox = cbv.getBoundingBox(); double V = ((bbox.xMax() - bbox.xMin()) * (bbox.yMax() - bbox.yMin()) * (bbox.zMax() - bbox.zMin())); double r = pow(V, 1.0 / 3.0) * 0.6; //double r = sceneLink->shapeNode->getBound().radius() * 0.5; zmpMarker->setRadius(r); zmpMarker->setCross(osg::Vec4(0.0, 1.0, 0.0, 1.0), r * 2.5, 2.0f); } cmMarker->setSize(visibleSceneLinks->getBound().radius()); } connections.add(bodyItem->sigUpdated().connect( bind(&SceneBodyImpl::updateMarkersAndManipulators, this))); connections.add(bodyItem->sigKinematicStateChanged().connect( bind(&SceneBodyImpl::onKinematicStateChanged, this))); connections.add(bodyItem->sigSelfCollisionsUpdated().connect( bind(&SceneBodyImpl::onSelfCollisionsUpdated, this))); onCollisionLinkHighlightModeChanged(); updateMarkersAndManipulators(); onKinematicStateChanged(); } void SceneBody::onDetachedFromScene() { impl->onDetachedFromScene(); } void SceneBodyImpl::onDetachedFromScene() { connections.disconnect(); changeCollisionLinkHighlightMode(false); } void SceneBodyImpl::onKinematicStateChanged() { if(TRACE_FUNCTIONS){ static int counter = 0; cout << "SceneBodyImpl::onKinematicStateChanged():" << counter++ << endl; } if(body){ int numLinks = body->numLinks(); for(int i=0; i < numLinks; ++i){ Link* link = body->link(i); const Vector3& p = link->p; const Matrix3& R = link->R; osg::Matrix m(R(0,0), R(1,0), R(2,0), 0.0, R(0,1), R(1,1), R(2,1), 0.0, R(0,2), R(1,2), R(2,2), 0.0, p(0), p(1), p(2), 1.0); sceneLinks[i]->setMatrix(m); } } if(isCmVisible){ cmMarker->setPosition(bodyItem->centerOfMass()); } if(isZmpVisible){ zmpMarker->setPosition(bodyItem->zmp()); } self->requestRedraw(); } void SceneBodyImpl::createOutline(SceneLink* sceneLink) { sceneLink->outline = new OsgOutlineFx(4.0); sceneLink->removeChild(sceneLink->shapeNode.get()); sceneLink->outline->addChild(sceneLink->shapeNode.get()); sceneLink->addChild(sceneLink->outline.get()); sceneLink->shapeNode = sceneLink->outline.get(); } bool SceneBodyImpl::setPointedLinkVisual(SceneLink* sceneLink, bool on) { bool outlineCreated = false; if(!sceneLink->isPointed && on){ if(!sceneLink->outline){ createOutline(sceneLink); outlineCreated = true; } sceneLink->outline->setColor(osg::Vec4(1.0, 0.0, 0.0, 1.0)); sceneLink->outline->setEnabled(true); sceneLink->isPointed = true; } else if(sceneLink->isPointed && !on){ if(sceneLink->isColliding){ sceneLink->outline->setColor(osg::Vec4(1.0, 1.0, 0.0, 1.0)); } else { sceneLink->outline->setEnabled(false); } sceneLink->isPointed = false; } return outlineCreated; } bool SceneBodyImpl::setWorldCollisionLinkVisual(SceneLink* sceneLink, bool on) { bool outlineCreated = false; if(!sceneLink->isColliding && on){ if(!sceneLink->isPointed){ if(!sceneLink->outline){ createOutline(sceneLink); outlineCreated = true; } sceneLink->outline->setColor(osg::Vec4(1.0, 1.0, 0.0, 1.0)); sceneLink->outline->setEnabled(true); } sceneLink->isColliding = true; } else if(sceneLink->isColliding && !on){ if(!sceneLink->isPointed){ sceneLink->outline->setEnabled(false); } sceneLink->isColliding = false; } return outlineCreated; } void SceneBodyImpl::createScribe(SceneLink* sceneLink) { sceneLink->scribe = new osgFX::Scribe(); sceneLink->scribe->setWireframeColor(osg::Vec4(1.0, 1.0, 0.0, 1.0)); sceneLink->removeChild(sceneLink->shapeNode.get()); sceneLink->scribe->addChild(sceneLink->shapeNode.get()); sceneLink->addChild(sceneLink->scribe.get()); sceneLink->shapeNode = sceneLink->scribe.get(); } bool SceneBodyImpl::setSelfCollisionLinkVisual(SceneLink* sceneLink, bool on) { bool scribeCreated = false; if(on){ if(!sceneLink->scribe){ createScribe(sceneLink); scribeCreated = true; } else if(!sceneLink->scribe->getEnabled()){ sceneLink->scribe->setEnabled(true); } } else { if(sceneLink->scribe.valid()){ sceneLink->scribe->setEnabled(false); } } return scribeCreated; } void SceneBodyImpl::onSelfCollisionsUpdated() { osgCollision->dirtyDisplayList(); } void SceneBodyImpl::onWorldCollisionLinkSetChanged() { bool fxCreated = false; const boost::dynamic_bitset<>& w = bodyItem->worldCollisionLinkBitSet; const boost::dynamic_bitset<>& s = bodyItem->selfCollisionLinkBitSet; for(size_t i=0; i < sceneLinks.size(); ++i){ fxCreated |= setWorldCollisionLinkVisual(sceneLinks[i].get(), (w[i] && !s[i])); fxCreated |= setSelfCollisionLinkVisual(sceneLinks[i].get(), s[i]); } if(fxCreated){ self->requestRedrawWhenEffectNodeCreated(); } else { self->requestRedraw(); } } void SceneBodyImpl::onCollisionLinkHighlightModeChanged() { changeCollisionLinkHighlightMode(kinematicsBar->isCollisionLinkHighlihtMode()); } void SceneBodyImpl::changeCollisionLinkHighlightMode(bool on) { if(!connectionToSigWorldCollisionLinkSetChanged.connected() && on){ connectionToSigWorldCollisionLinkSetChanged = bodyItem->sigWorldCollisionLinkSetChanged().connect( bind(&SceneBodyImpl::onWorldCollisionLinkSetChanged, this)); onWorldCollisionLinkSetChanged(); } else if(connectionToSigWorldCollisionLinkSetChanged.connected() && !on){ connectionToSigWorldCollisionLinkSetChanged.disconnect(); for(size_t i=0; i < sceneLinks.size(); ++i){ setWorldCollisionLinkVisual(sceneLinks[i].get(), false); setSelfCollisionLinkVisual(sceneLinks[i].get(), false); } self->requestRedraw(); } } void SceneBody::setLinkVisibilities(const boost::dynamic_bitset<>& visibilities) { impl->setLinkVisibilities(visibilities); } void SceneBodyImpl::setLinkVisibilities(const boost::dynamic_bitset<>& visibilities) { visibleSceneLinks->removeChildren(0, visibleSceneLinks->getNumChildren()); int n = std::min(visibilities.size(), sceneLinks.size()); for(int i=0; i < n; ++i){ if(visibilities[i] && sceneLinks[i]->isValid){ visibleSceneLinks->addChild(sceneLinks[i].get()); } } self->requestRedraw(); } void SceneBody::showCenterOfMass(bool on) { impl->showCenterOfMass(on); } void SceneBodyImpl::showCenterOfMass(bool on) { isCmVisible = on; if(on){ markerGroup->addChild(cmMarker.get()); cmMarker->setPosition(bodyItem->centerOfMass()); } else { markerGroup->removeChild(cmMarker.get()); } self->requestRedraw(); } bool SceneBody::isCenterOfMassVisible() const { return impl->isCmVisible; } void SceneBody::showZmp(bool on) { impl->showZmp(on); } void SceneBodyImpl::showZmp(bool on) { isZmpVisible = on; if(on){ markerGroup->addChild(zmpMarker.get()); zmpMarker->setPosition(bodyItem->zmp()); } else { markerGroup->removeChild(zmpMarker.get()); } self->requestRedraw(); } bool SceneBody::isZmpVisible() const { return impl->isZmpVisible; } /** If the return value is PT_SCENE_LINK member variable 'pointedSceneLink' is updated with the pointed SceneLink object. If the pointed object is AttitudeDragger, the function returns PT_SCENE_LINK and member variable 'rotationAxis' is also set. Otherwise, 'pointedSceneLink' is set to 0. */ SceneBodyImpl::PointedType SceneBodyImpl::findPointedObject(const osg::NodePath& path) { PointedType pointedType = PT_NONE; pointedSceneLink = 0; rotationAxis = NOAXIS; const int n = path.size(); bool pathContainsSelf = false; for(int i=0; i < n; ++i){ if(!pathContainsSelf){ if(dynamic_cast(path[i]) == self){ pathContainsSelf = true; } continue; } SceneLink* sceneLink = dynamic_cast(path[i]); if(sceneLink){ pointedSceneLink = sceneLink; pointedType = PT_SCENE_LINK; if(i < n - 2){ AttitudeDragger* dragger = dynamic_cast(path[i+1]); if(dragger){ const string& axis = path[i+2]->getName(); if(axis == "x"){ rotationAxis = X; } else if(axis == "y"){ rotationAxis = Y; } else if(axis == "z"){ rotationAxis = Z; } } } break; } SphereMarker* marker = dynamic_cast(path[i]); if(marker == zmpMarker.get()){ pointedType = PT_ZMP; break; } } return pointedType; } void SceneBodyImpl::setOutlineForPointedLink(SceneLink* sceneLink) { if(outlinedLink == sceneLink){ return; } if(outlinedLink){ setPointedLinkVisual(outlinedLink, false); outlinedLink = 0; } bool outlineCreated = false; if(sceneLink){ outlinedLink = sceneLink; outlineCreated = setPointedLinkVisual(outlinedLink, true); } if(outlineCreated){ self->requestRedrawWhenEffectNodeCreated(); } else { self->requestRedraw(); } } void SceneBodyImpl::updateMarkersAndManipulators() { bool show = (self->sceneMode() == SceneObject::EDIT_MODE && self->isEditable()); Link* baseLink = bodyItem->currentBaseLink(); PinDragIKptr pin = bodyItem->pinDragIK(); for(size_t i=0; i < sceneLinks.size(); ++i){ SceneLink* sceneLink = sceneLinks[i].get(); Link* link = sceneLink->link; sceneLink->hideMarker(); if(show){ if(link == baseLink){ sceneLink->showMarker(osg::Vec4(1.0f, 0.1f, 0.1f, 0.4f), 0.02); } else { int pinAxes = pin->pinAxes(link); if(pinAxes & (InverseKinematics::TRANSFORM_6D)){ sceneLink->showMarker(osg::Vec4(1.0f, 1.0f, 0.1f, 0.4f), 0.02); } } } } attitudeDragger->detach(); if(show && targetLink && (kinematicsBar->mode() != KinematicsBar::FK_MODE) && kinematicsBar->isAttitudeMode()){ attitudeDragger->attachTo(sceneLinks[targetLink->index].get()); } self->requestRedraw(); } void SceneBodyImpl::makeLinkFree(SceneLink* sceneLink) { Link* link = sceneLink->link; if(bodyItem->currentBaseLink() == link){ bodyItem->setCurrentBaseLink(0); } bodyItem->pinDragIK()->setPin(link, InverseKinematics::NO_AXES); bodyItem->notifyUpdate(); } void SceneBodyImpl::setBaseLink(SceneLink* sceneLink) { bodyItem->setCurrentBaseLink(sceneLink->link); bodyItem->notifyUpdate(); } void SceneBodyImpl::toggleBaseLink(SceneLink* sceneLink) { Link* baseLink = bodyItem->currentBaseLink(); if(sceneLink->link != baseLink){ bodyItem->setCurrentBaseLink(sceneLink->link); } else { bodyItem->setCurrentBaseLink(0); } bodyItem->notifyUpdate(); } void SceneBodyImpl::togglePin(SceneLink* sceneLink, bool toggleTranslation, bool toggleRotation) { Link* link = sceneLink->link; PinDragIKptr pin = bodyItem->pinDragIK(); InverseKinematics::AxisSet axes = pin->pinAxes(link); if(toggleTranslation && toggleRotation){ if(axes == InverseKinematics::NO_AXES){ axes = InverseKinematics::TRANSFORM_6D; } else { axes = InverseKinematics::NO_AXES; } } else { if(toggleTranslation){ axes = (InverseKinematics::AxisSet)(axes ^ InverseKinematics::TRANSLATION_3D); } if(toggleRotation){ axes = (InverseKinematics::AxisSet)(axes ^ InverseKinematics::ROTATION_3D); } } pin->setPin(link, axes); bodyItem->notifyUpdate(); } bool SceneBody::onKeyPressEvent(const SceneViewEvent& event) { return impl->onKeyPressEvent(event); } bool SceneBodyImpl::onKeyPressEvent(const SceneViewEvent& event) { if(!outlinedLink){ return false; } bool handled = true; int key = event.key(); if(islower(key) == 0){ key = toupper(key); } switch(key){ case 'B': toggleBaseLink(outlinedLink); break; case 'R': togglePin(outlinedLink, false, true); break; case 'T': togglePin(outlinedLink, true, false); break; default: handled = false; break; } return handled; } bool SceneBody::onKeyReleaseEvent(const SceneViewEvent& event) { return impl->onKeyReleaseEvent(event); } bool SceneBodyImpl::onKeyReleaseEvent(const SceneViewEvent& event) { return false; } bool SceneBody::onButtonPressEvent(const SceneViewEvent& event) { return impl->onButtonPressEvent(event); } bool SceneBodyImpl::onButtonPressEvent(const SceneViewEvent& event) { bool handled = false; isDragging = false; PointedType pointedType = findPointedObject(event.path()); setOutlineForPointedLink(pointedSceneLink); if(pointedType == PT_SCENE_LINK){ if(event.button() == GUIEventAdapter::LEFT_MOUSE_BUTTON){ targetLink = pointedSceneLink->link; updateMarkersAndManipulators(); ik.reset(); switch(kinematicsBar->mode()){ case KinematicsBar::AUTO_MODE: ik = body->getDefaultIK(targetLink); if(ik){ startIK(event); } else { startFK(event); } break; case KinematicsBar::FK_MODE: if(targetLink == bodyItem->currentBaseLink()){ startIK(event); } else { startFK(event); } break; case KinematicsBar::IK_MODE: startIK(event); break; } } else if(event.button() == GUIEventAdapter::MIDDLE_MOUSE_BUTTON){ togglePin(pointedSceneLink, true, true); } else if(event.button() == GUIEventAdapter::RIGHT_MOUSE_BUTTON){ } handled = true; } else if(pointedType == PT_ZMP){ startZmpTranslation(event); handled = true; } if(dragMode != DRAG_NONE && outlinedLink){ setPointedLinkVisual(outlinedLink, false); self->requestRedraw(); } return handled; } bool SceneBody::onButtonReleaseEvent(const SceneViewEvent& event) { return impl->onButtonReleaseEvent(event); } bool SceneBodyImpl::onButtonReleaseEvent(const SceneViewEvent& event) { isDragging = false; if(dragMode != DRAG_NONE){ bodyItem->acceptKinematicStateEdit(); dragMode = DRAG_NONE; if(outlinedLink){ setPointedLinkVisual(outlinedLink, true); self->requestRedraw(); } return true; } return false; } bool SceneBody::onDoubleClickEvent(const SceneViewEvent& event) { return impl->onDoubleClickEvent(event); } bool SceneBodyImpl::onDoubleClickEvent(const SceneViewEvent& event) { if(findPointedObject(event.path()) == PT_SCENE_LINK){ if(event.button() == GUIEventAdapter::LEFT_MOUSE_BUTTON){ if(BodyBar::instance()->makeSingleSelection(bodyItem)){ LinkSelectionView::mainInstance()->makeSingleSelection(bodyItem, pointedSceneLink->link->index); } return true; } } return false; } bool SceneBody::onPointerMoveEvent(const SceneViewEvent& event) { return impl->onPointerMoveEvent(event); } bool SceneBodyImpl::onPointerMoveEvent(const SceneViewEvent& event) { if(TRACE_FUNCTIONS){ os << "SceneBodyImpl::onPointerMoveEvent()" << endl; } if(dragMode == DRAG_NONE){ findPointedObject(event.path()); setOutlineForPointedLink(pointedSceneLink); static format f(_("%1% / %2%")); if(pointedSceneLink){ event.updateIndicator(str(f % bodyItem->name() % pointedSceneLink->link->name())); } else { event.updateIndicator(""); } } else { if(!isDragging){ bodyItem->beginKinematicStateEdit(); isDragging = true; } switch(dragMode){ case LINK_IK_TRANSLATION: case LINK_IK_ROTATION: dragIK(event); break; case LINK_FK_ROTATION: dragFKRotation(event); break; case ZMP_TRANSLATION: dragZmpTranslation(event); break; default: break; } } return true; } void SceneBody::onPointerLeaveEvent(const SceneViewEvent& event) { impl->onPointerLeaveEvent(event); } void SceneBodyImpl::onPointerLeaveEvent(const SceneViewEvent& event) { setOutlineForPointedLink(0); } void SceneBody::onContextMenuRequest(const SceneViewEvent& event, MenuManager& menuManager) { impl->onContextMenuRequest(event, menuManager); } void SceneBodyImpl::onContextMenuRequest(const SceneViewEvent& event, MenuManager& menuManager) { PointedType pointedType = findPointedObject(event.path()); if(bodyItem && pointedType == PT_SCENE_LINK){ menuManager.addItem(_("Set Free"))->sigTriggered().connect( bind(&SceneBodyImpl::makeLinkFree, this, pointedSceneLink)); menuManager.addItem(_("Set Base"))->sigTriggered().connect( bind(&SceneBodyImpl::setBaseLink, this, pointedSceneLink)); menuManager.addItem(_("Set Translation Pin"))->sigTriggered().connect( bind(&SceneBodyImpl::togglePin, this, pointedSceneLink, true, false)); menuManager.addItem(_("Set Rotation Pin"))->sigTriggered().connect( bind(&SceneBodyImpl::togglePin, this, pointedSceneLink, false, true)); menuManager.addItem(_("Set Both Pins"))->sigTriggered().connect( bind(&SceneBodyImpl::togglePin, this, pointedSceneLink, true, true)); menuManager.addSeparator(); menuManager.addItem(_("Level Attitude"))->sigTriggered().connect( bind(&SceneBodyImpl::makeLinkAttitudeLevel, this)); menuManager.addSeparator(); menuManager.setPath(N_("/Markers")); Action* item = menuManager.addCheckItem(_("Center of Mass")); item->setChecked(isCmVisible); item->sigToggled().connect(bind(&SceneBodyImpl::showCenterOfMass, this, _1)); item = menuManager.addCheckItem(_("ZMP")); item->setChecked(isZmpVisible); item->sigToggled().connect(bind(&SceneBodyImpl::showZmp, this, _1)); } } void SceneBody::onSceneModeChanged() { impl->onSceneModeChanged(); } void SceneBodyImpl::onSceneModeChanged() { if(self->sceneMode() == SceneObject::VIEW_MODE && outlinedLink){ setOutlineForPointedLink(0); } updateMarkersAndManipulators(); } bool SceneBody::onUndoRequest() { return impl->bodyItem->undoKinematicState(); } bool SceneBody::onRedoRequest() { return impl->bodyItem->redoKinematicState(); } void SceneBodyImpl::makeLinkAttitudeLevel() { if(pointedSceneLink){ Link* targetLink = pointedSceneLink->link; InverseKinematicsPtr ik = bodyItem->getCurrentIK(targetLink); if(ik){ const Matrix3& R = targetLink->R; const double theta = acos(R(2, 2)); const Vector3 z(R(0,2), R(1, 2), R(2, 2)); const Vector3 axis = z.cross(Vector3::UnitZ()).normalized(); const Matrix3 R2 = AngleAxisd(theta, axis) * R; bodyItem->beginKinematicStateEdit(); if(ik->calcInverseKinematics(targetLink->p, R2)){ bodyItem->notifyKinematicStateChange(true); bodyItem->acceptKinematicStateEdit(); } } } } void SceneBodyImpl::setPlaneProjector(const SceneViewEvent& event) { osg::Vec3 eye, center, up; event.camera()->getViewMatrixAsLookAt(eye, center, up); projector = new osgManipulator::PlaneProjector(osg::Plane(eye - center, event.point())); pointerInfo.reset(); pointerInfo.setCamera(event.camera()); } /** \todo use BodyItem::getCurrentIK(); */ void SceneBodyImpl::startIK(const SceneViewEvent& event) { orgPointerPos << event.point().x(), event.point().y(), event.point().z(); orgTargetLinkPos = targetLink->p; Link* baseLink = bodyItem->currentBaseLink(); if(!ik){ if(bodyItem->pinDragIK()->numPinnedLinks() == 0 && baseLink){ ikPath = body->getJointPath(baseLink, targetLink); if(ikPath){ if(!ikPath->hasAnalyticalIK()){ //ikPath->setBestEffortIKMode(true); } ik = ikPath; } } } if(!ik){ pinDragIK = bodyItem->pinDragIK(); pinDragIK->setBaseLink(baseLink); pinDragIK->setTargetLink(targetLink, kinematicsBar->isAttitudeMode()); if(pinDragIK->initialize()){ ik = pinDragIK; } } if(ik){ projector = 0; if(kinematicsBar->isAttitudeMode()){ if(rotationAxis >= X){ orgAttitude = targetLink->R; const Matrix3& R = orgAttitude; axis = R.col(rotationAxis); const Vector3 arm = orgPointerPos - (axis.dot(orgPointerPos - orgTargetLinkPos) * axis + orgTargetLinkPos); rotationBaseX = arm.normalized(); rotationBaseY = axis.cross(arm).normalized(); const osg::Vec3 center(orgTargetLinkPos[0], orgTargetLinkPos[1], orgTargetLinkPos[2]); const float radius = arm.norm(); osg::Cylinder* cylinder = new osg::Cylinder(center, radius, std::numeric_limits::max()); osg::Quat rot; rot.makeRotate(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(axis[0], axis[1], axis[2])); cylinder->setRotation(rot); projector = new osgManipulator::CylinderProjector(cylinder); pointerInfo.reset(); pointerInfo.setCamera(event.camera()); dragMode = LINK_IK_ROTATION; } } if(!projector){ setPlaneProjector(event); dragMode = LINK_IK_TRANSLATION; } fkTraverse.find(baseLink ? baseLink : body->rootLink(), true, true); /* isSnapping = false; snapFootLinkInfos.clear(); if(kinematicsBar->isFootSnapMode()){ setupFootSnap(); } */ if(kinematicsBar->isPenetrationBlockMode()){ penetrationBlocker = bodyItem->createPenetrationBlocker(targetLink, true); } else { penetrationBlocker.reset(); } } self->requestRedraw(); } /** \todo This function should be implemented in the PoseSeq plugin */ /* void SceneBodyImpl::setupFootSnap() { bool isFootLink = false; const vector& feet = bodyItem->footInfos; for(size_t i=0; i < feet.size(); ++i){ if(feet[i].link == targetLink){ isFootLink = true; break; } } if(isFootLink){ PoseSeqItem* poseSeqItem = ItemTreeView::mainInstance()->selectedSubItem(bodyItem, true); if(!poseSeqItem){ BodyMotionItem* motionItem = ItemTreeView::mainInstance()->selectedSubItem(bodyItem, true); if(motionItem){ poseSeqItem = motionItem->findOwnerItem(); } } if(poseSeqItem){ PoseSeqPtr pseq = poseSeqItem->poseSeq(); TimeBar* timeBar = TimeBar::instance(); double time = timeBar->time(); /// \todo cache the previous iterator PoseSeq::iterator p = pseq->seek(pseq->begin(), time); if(p != pseq->end()){ double ts = timeBar->timeStep(); PoseSeq::iterator prev = p; do { if(fabs(prev->time() - time) >= ts){ PosePtr pose = prev->get(); if(pose){ Pose::LinkInfo* linkInfo = pose->ikLinkInfo(targetLink->index); if(linkInfo){ snapFootLinkInfos.push_back(linkInfo); break; } } } } while(prev-- != pseq->begin()); PoseSeq::iterator next = ++p; while(next != pseq->end()){ if(fabs(next->time() - time) >= ts){ PosePtr pose = next->get(); if(pose){ Pose::LinkInfo* linkInfo = pose->ikLinkInfo(targetLink->index); if(linkInfo){ snapFootLinkInfos.push_back(linkInfo); break; } } } ++next; } } } } } */ void SceneBodyImpl::dragIK(const SceneViewEvent& event) { Vec3ForProjection p; pointerInfo.setMousePosition(event.x(), event.y()); if(projector->project(pointerInfo, p)){ Vector3 newPos; Matrix3 newR; if(dragMode == LINK_IK_TRANSLATION){ newPos = (orgTargetLinkPos + (Vector3(p.x(), p.y(), p.z()) - orgPointerPos)); newR = targetLink->R; if(penetrationBlocker){ penetrationBlocker->adjust(newPos, newR, newPos - targetLink->p); } /* if(!snapFootLinkInfos.empty()){ snapFoot(newPos, newR); } */ } else if(dragMode == LINK_IK_ROTATION){ const Vector3 r = Vector3(p.x(), p.y(), p.z()) - orgTargetLinkPos; const double angle = atan2(r.dot(rotationBaseY), r.dot(rotationBaseX)); newR = AngleAxisd(angle, axis) * orgAttitude; newPos = orgTargetLinkPos; } if(ik->calcInverseKinematics(newPos, newR)){ fkTraverse.calcForwardKinematics(); bodyItem->notifyKinematicStateChange(true); } } } /* void SceneBodyImpl::snapFoot(Vector3& p, Matrix33& R) { double distanceThresh, angleThresh; kinematicsBar->getSnapThresholds(distanceThresh, angleThresh); double eMin = std::numeric_limits::max(); for(size_t i=0; i < snapFootLinkInfos.size(); ++i){ Pose::LinkInfo* info = snapFootLinkInfos[i]; double eT = norm2(info->p - p); // * 40.0; // 2.5cm double eR = norm2(omegaFromRot(Matrix33(trans(R) * info->R))); // * 2.0; // about 30 deg if(eT < distanceThresh && eR < angleThresh){ double eTotal = eT * 40.0 + eR * 2.0; if(eTotal < eMin){ if(!isSnapping){ footAttitudeBeforeSnap = R; } p = info->p; R = info->R; eMin = eTotal; isSnapping = true; } } } if(isSnapping && eMin == std::numeric_limits::max()){ R = footAttitudeBeforeSnap; isSnapping = false; } } */ void SceneBodyImpl::startFK(const SceneViewEvent& event) { if(targetLink->jointType == Link::ROTATIONAL_JOINT){ orgJointPosition = targetLink->q; const Vector3 pos(event.point().x(), event.point().y(), event.point().z()); const Vector3 axis = targetLink->R * targetLink->a; const Vector3 arm = pos - (axis.dot(pos - targetLink->p) * axis + targetLink->p); if(arm.norm() > 1.0e-6){ rotationBaseX = arm.normalized(); rotationBaseY = axis.cross(rotationBaseX); osg::Vec3 eye, center, up; event.camera()->getViewMatrixAsLookAt(eye, center, up); osg::Vec3 eyeDirection(center - eye); eyeDirection.normalize(); osg::Vec3d osgAxis(axis[0], axis[1], axis[2]); if(fabs(osgAxis * eyeDirection) > 0.7){ projector = new osgManipulator::PlaneProjector(osg::Plane(osg::Vec3d(axis[0], axis[1], axis[2]), event.point())); } else { const float radius = arm.norm(); const osg::Vec3 center(targetLink->p[0], targetLink->p[1], targetLink->p[2]); osg::Cylinder* cylinder = new osg::Cylinder(center, radius, std::numeric_limits::max()); osg::Quat rot; rot.makeRotate(osg::Vec3d(0.0, 0.0, 1.0), osgAxis); cylinder->setRotation(rot); projector = new osgManipulator::CylinderProjector(cylinder); } pointerInfo.reset(); pointerInfo.setCamera(event.camera()); dragMode = LINK_FK_ROTATION; } } } void SceneBodyImpl::dragFKRotation(const SceneViewEvent& event) { Vec3ForProjection pos0; pointerInfo.setMousePosition(event.x(), event.y()); if(projector->project(pointerInfo, pos0)){ const Vector3 p = Vector3(pos0.x(), pos0.y(), pos0.z()) - targetLink->p; const double x = p.dot(rotationBaseX); const double y = p.dot(rotationBaseY); const double angle = atan2(y, x); targetLink->q = orgJointPosition + angle; bodyItem->notifyKinematicStateChange(true); } } void SceneBodyImpl::startZmpTranslation(const SceneViewEvent& event) { orgPointerPos << event.point().x(), event.point().y(), event.point().z(); osg::Plane plane(osg::Vec3(0.0, 0.0, 1.0), event.point()); projector = new osgManipulator::PlaneProjector(plane); pointerInfo.reset(); pointerInfo.setCamera(event.camera()); orgZmpPos = bodyItem->zmp(); dragMode = ZMP_TRANSLATION; } void SceneBodyImpl::dragZmpTranslation(const SceneViewEvent& event) { Vec3ForProjection p; pointerInfo.setMousePosition(event.x(), event.y()); if(projector->project(pointerInfo, p)){ bodyItem->setZmp(orgZmpPos + (Vector3(p.x(), p.y(), p.z()) - orgPointerPos)); bodyItem->notifyKinematicStateChange(true); } } choreonoid-1.1.0+dfsg/src/BodyPlugin/SceneBody.h000066400000000000000000000034021207742442300214540ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_SCENE_BODY_H_INCLUDED #define CNOID_BODYPLUGIN_SCENE_BODY_H_INCLUDED #include #include #include "BodyItem.h" #include "exportdecl.h" namespace cnoid { class SceneBodyImpl; class CNOID_EXPORT SceneBody : public SceneObject { public: SceneBody(BodyItemPtr bodyItem); void setLinkVisibilities(const boost::dynamic_bitset<>& visibilities); void showCenterOfMass(bool on); bool isCenterOfMassVisible() const; void showZmp(bool on); bool isZmpVisible() const; Link* getPointedSceneLink(); osg::ref_ptr getPointedShapeNode(); protected: virtual ~SceneBody(); bool createSceneLinks(); virtual void onAttachedToScene(); virtual void onDetachedFromScene(); virtual bool onKeyPressEvent(const SceneViewEvent& event); virtual bool onKeyReleaseEvent(const SceneViewEvent& event); virtual bool onButtonPressEvent(const SceneViewEvent& event); virtual bool onButtonReleaseEvent(const SceneViewEvent& event); virtual bool onDoubleClickEvent(const SceneViewEvent& event); virtual bool onPointerMoveEvent(const SceneViewEvent& event); virtual void onPointerLeaveEvent(const SceneViewEvent& event); virtual void onContextMenuRequest(const SceneViewEvent& event, MenuManager& menuManager); virtual void onSceneModeChanged(); virtual bool onUndoRequest(); virtual bool onRedoRequest(); private: SceneBodyImpl* impl; friend class SceneBodyImpl; }; typedef osg::ref_ptr SceneBodyPtr; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/SceneBodyManager.cpp000066400000000000000000000212061207742442300233040ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "SceneBodyManager.h" #include "SceneBody.h" #include "BodyItem.h" #include "LinkSelectionView.h" #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; SceneBodyManager* instance_ = 0; } namespace cnoid { class SBMImpl : public boost::signals::trackable { public: SBMImpl(ExtensionManager& ext); struct SceneBodyInfo { BodyItem* bodyItem; SceneBodyPtr sceneBody; bool isShown; signals::connection cSigDetachedFromRoot; signals::connection cSigCheckToggled; signals::connection cSigLinkSelectionChanged; ~SceneBodyInfo(){ cSigDetachedFromRoot.disconnect(); cSigCheckToggled.disconnect(); cSigLinkSelectionChanged.disconnect(); } }; typedef list< boost::function > FactoryList; FactoryList factories; typedef map SceneBodyInfoMap; SceneBodyInfoMap sceneBodyInfoMap; SceneView* sceneView; ItemTreeView* itemTreeView; Action* onlySelectedLinkCheck; void onItemAdded(Item* item); void onBodyItemDetached(BodyItem* item); void showBodyItem(SceneBodyInfo* info, bool show); void onOnlySelectedLinkToggled(); void onLinkSelectionChanged(SceneBodyInfo* info); bool store(Archive& archive); void restore(const Archive& archive); }; class FactoryHolderImpl : public SceneBodyManager::FactoryHolder { public: FactoryHolderImpl(SBMImpl::FactoryList& factories, SBMImpl::FactoryList::iterator iter) : factories(factories), iter(iter) { } ~FactoryHolderImpl() { factories.erase(iter); } private: SBMImpl::FactoryList& factories; SBMImpl::FactoryList::iterator iter; }; } SceneBodyManager* SceneBodyManager::instance() { return instance_; } SceneBodyManager::SceneBodyManager(ExtensionManager& ext) { assert(!instance_); impl = new SBMImpl(ext); instance_ = this; } SBMImpl::SBMImpl(ExtensionManager& ext) { sceneView = SceneView::mainInstance(); RootItem::mainInstance()->sigItemAdded().connect(bind(&SBMImpl::onItemAdded, this, _1)); itemTreeView = ItemTreeView::mainInstance(); ext.connectProjectArchiver( "SceneBodyManager", bind(&SBMImpl::store, this, _1), bind(&SBMImpl::restore, this, _1)); onlySelectedLinkCheck = ext.menuManager().setPath("/Options/Scene View").addCheckItem(_("Show only selected links")); onlySelectedLinkCheck->setChecked(false); onlySelectedLinkCheck->sigToggled().connect(bind(&SBMImpl::onOnlySelectedLinkToggled, this)); } SceneBodyManager::~SceneBodyManager() { delete impl; } /** @param factory A factory function object that creates a scene body instance of a customized sub class. In order to avoid the collision between the multiple factories, it is desirable that the factory function checks the actual type of a BodyItem instance and a customized scene body is only returned if the type matches. Example code is as follows: \code SceneBody* factory(BodyItem* bodyItem) { BodyItemEx* ex = dynamic_cast(bodyItem); if(ex){ return new SceneBodyEx(ex); } return 0; } \endcode @return A holder object of the registered factory. The destructor of the holder object unregisters the factory. This object is usually passed to 'ExtensionManager::manage()' function so that the factory can be automatically removed when the plugin is finalized. */ SceneBodyManager::FactoryHolder* SceneBodyManager::addSceneBodyFactory(boost::function factory) { impl->factories.push_front(factory); return new FactoryHolderImpl(impl->factories, --impl->factories.end()); } void SBMImpl::onItemAdded(Item* item) { if(TRACE_FUNCTIONS){ cout << "SBMImpl::onItemAdded()" << endl; } if(BodyItem* bodyItem = dynamic_cast(item)){ SceneBodyInfo* info = &sceneBodyInfoMap[bodyItem]; info->bodyItem = bodyItem; if(!info->sceneBody){ for(FactoryList::iterator p = factories.begin(); p != factories.end(); ++p){ info->sceneBody = (*p)(bodyItem); if(info->sceneBody.valid()){ break; } } } if(!info->sceneBody){ info->sceneBody = new SceneBody(bodyItem); } info->isShown = false; info->cSigDetachedFromRoot = bodyItem->sigDetachedFromRoot().connect( bind(&SBMImpl::onBodyItemDetached, this, bodyItem)); info->cSigCheckToggled = itemTreeView->sigCheckToggled(bodyItem).connect( bind(&SBMImpl::showBodyItem, this, info, _1)); if(itemTreeView->isItemChecked(bodyItem)){ showBodyItem(info, true); } } } void SBMImpl::onBodyItemDetached(BodyItem* item) { if(TRACE_FUNCTIONS){ cout << "SBMImpl::onBodyItemDetached()" << endl; } SceneBodyInfoMap::iterator p = sceneBodyInfoMap.find(item); if(p != sceneBodyInfoMap.end()){ SceneBodyInfo* info = &p->second; showBodyItem(info, false); sceneBodyInfoMap.erase(p); } } void SBMImpl::showBodyItem(SceneBodyInfo* info, bool show) { if(TRACE_FUNCTIONS){ cout << "SBMImpl::showBodyItem()" << endl; } if(info->isShown && !show){ info->cSigLinkSelectionChanged.disconnect(); sceneView->removeSceneObject(info->sceneBody.get()); info->isShown = false; sceneView->requestRedraw(); } else if(!info->isShown && show){ sceneView->addSceneObject(info->sceneBody.get()); info->isShown = true; info->cSigLinkSelectionChanged = LinkSelectionView::mainInstance()->sigSelectionChanged(info->bodyItem).connect( bind(&SBMImpl::onLinkSelectionChanged, this, info)); onLinkSelectionChanged(info); sceneView->requestRedraw(); } } void SBMImpl::onOnlySelectedLinkToggled() { SceneBodyInfoMap::iterator p; for(p = sceneBodyInfoMap.begin(); p != sceneBodyInfoMap.end(); ++p){ if(onlySelectedLinkCheck->isChecked()){ onLinkSelectionChanged(&p->second); } else { dynamic_bitset<> visibilities(p->first->body()->numLinks(), true); p->second.sceneBody->setLinkVisibilities(visibilities); } } } void SBMImpl::onLinkSelectionChanged(SceneBodyInfo* info) { if(onlySelectedLinkCheck->isChecked()){ info->sceneBody->setLinkVisibilities( LinkSelectionView::mainInstance()->getLinkSelection(info->bodyItem)); } } bool SBMImpl::store(Archive& archive) { YamlSequencePtr states = new YamlSequence(); SceneBodyInfoMap::iterator p; for(p = sceneBodyInfoMap.begin(); p != sceneBodyInfoMap.end(); ++p){ BodyItem* bodyItem = p->first.get(); int id = archive.getItemId(bodyItem); if(id >= 0){ SceneBodyPtr sceneBody = p->second.sceneBody; YamlMappingPtr state = new YamlMapping(); state->write("bodyItem", id); state->write("editable", sceneBody->isEditable()); state->write("centerOfMass", sceneBody->isCenterOfMassVisible()); state->write("zmp", sceneBody->isZmpVisible()); states->append(state); } } if(!states->empty()){ archive.insert("sceneBodies", states); return true; } return false; } void SBMImpl::restore(const Archive& archive) { YamlSequence& states = *archive["sceneBodies"].toSequence(); for(int i=0; i < states.size(); ++i){ YamlMapping* state = states[i].toMapping(); BodyItem* bodyItem = archive.findItem(state->get("bodyItem", -1)); if(bodyItem){ SceneBodyInfoMap::iterator p = sceneBodyInfoMap.find(bodyItem); if(p != sceneBodyInfoMap.end()){ SceneBodyPtr sceneBody = p->second.sceneBody; sceneBody->setEditable(state->get("editable", sceneBody->isEditable())); sceneBody->showCenterOfMass(state->get("centerOfMass", sceneBody->isCenterOfMassVisible())); sceneBody->showZmp(state->get("zmp", sceneBody->isZmpVisible())); } } } } choreonoid-1.1.0+dfsg/src/BodyPlugin/SceneBodyManager.h000066400000000000000000000014071207742442300227520ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_SCENE_BODY_MANAGER_H_INCLUDED #define CNOID_BODYPLUGIN_SCENE_BODY_MANAGER_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { class SceneBody; class BodyItem; class SBMImpl; class CNOID_EXPORT SceneBodyManager { public: static SceneBodyManager* instance(); SceneBodyManager(ExtensionManager& ext); ~SceneBodyManager(); class FactoryHolder { public: virtual ~FactoryHolder() { } }; FactoryHolder* addSceneBodyFactory(boost::function factory); private: SBMImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/SceneWorld.cpp000066400000000000000000000073231207742442300222070ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "SceneWorld.h" #include "OsgCollision.h" #include "WorldItem.h" #include #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } SceneWorld::SceneWorld(WorldItemPtr worldItem) : os(MessageView::mainInstance()->cout()), worldItem(worldItem) { if(TRACE_FUNCTIONS){ cout << "SceneWorld::SceneWorld()" << endl; } osgCollision = new OsgCollision(); osgCollision->setColdetPairs(worldItem->coldetPairs); osg::Geode* geode = new osg::Geode(); geode->addDrawable(osgCollision.get()); addChild(geode); } SceneWorld::~SceneWorld() { if(TRACE_FUNCTIONS){ cout << "SceneWorld::~SceneWorld()" << endl; } } void SceneWorld::onAttachedToScene() { if(TRACE_FUNCTIONS){ cout << "SceneWorld::onAttachedToScene()" << endl; } connectionWithSigCollisionsUpdated = worldItem->sigCollisionsUpdated().connect( bind(&SceneWorld::onCollisionsUpdated, this)); } void SceneWorld::onDetachedFromScene() { if(TRACE_FUNCTIONS){ cout << "SceneWorld::onDetachedFromScene()" << endl; } connectionWithSigCollisionsUpdated.disconnect(); } void SceneWorld::onCollisionsUpdated() { osgCollision->dirtyDisplayList(); requestRedraw(); } SceneWorldManager::SceneWorldManager() : os(MessageView::mainInstance()->cout()) { if(TRACE_FUNCTIONS){ cout << "SceneWorldManager::SceneWorldManager()" << endl; } itemTreeView = ItemTreeView::mainInstance(); sceneView = SceneView::mainInstance(); RootItem::mainInstance()->sigItemAdded().connect( bind(&SceneWorldManager::onItemAdded, this, _1)); } SceneWorldManager::~SceneWorldManager() { } void SceneWorldManager::onItemAdded(Item* item) { if(TRACE_FUNCTIONS){ cout << "SceneWorldManager::onItemAdded()" << endl; } if(WorldItem* worldItem = dynamic_cast(item)){ worldItem->sigDetachedFromRoot().connect( bind(&SceneWorldManager::onWorldItemDetached, this, worldItem)); itemTreeView->sigCheckToggled(worldItem).connect( bind(&SceneWorldManager::onWorldItemCheckToggled, this, worldItem, _1)); if(itemTreeView->isItemChecked(worldItem)){ showSceneWorld(worldItem, true); } } } /** @todo do disconnecting operation */ void SceneWorldManager::onWorldItemDetached(WorldItem* worldItem) { if(TRACE_FUNCTIONS){ cout << "SceneWorldManager::onWorldItemDetached()" << endl; } showSceneWorld(worldItem, false); } void SceneWorldManager::onWorldItemCheckToggled(WorldItem* worldItem, bool isChecked) { if(TRACE_FUNCTIONS){ cout << "SceneWorldManager::onWorldItemShown()" << endl; } showSceneWorld(worldItem, isChecked); } void SceneWorldManager::showSceneWorld(WorldItem* worldItem, bool show) { if(TRACE_FUNCTIONS){ cout << "SceneWorldManager::showSceneWorld()" << endl; } SceneWorldMap::iterator p = sceneWorlds.find(worldItem); if(!show && p != sceneWorlds.end()){ sceneView->removeSceneObject(p->second.get()); sceneWorlds.erase(p); sceneView->requestRedraw(); } else if(show && p == sceneWorlds.end()){ SceneWorldPtr sceneWorld = new SceneWorld(worldItem); sceneWorlds[worldItem] = sceneWorld; sceneView->addSceneObject(sceneWorld.get()); sceneView->requestRedraw(); } } choreonoid-1.1.0+dfsg/src/BodyPlugin/SceneWorld.h000066400000000000000000000033641207742442300216550ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_SCENE_WORLD_H_INCLUDED #define CNOID_BODYPLUGIN_SCENE_WORLD_H_INCLUDED #include #include #include #include #include "WorldItem.h" #include "OsgCollision.h" #include "exportdecl.h" namespace cnoid { class SceneView; class ItemTreeView; /** The role of this class is the visualization of the WorldItem state. The bodies contained in a world are visualized by the SceneBody class, so that this class visualizes the remaining objects such as the collisions between bodies. */ class CNOID_EXPORT SceneWorld : public SceneObject { public: SceneWorld(WorldItemPtr worldItem); protected: virtual ~SceneWorld(); virtual void onAttachedToScene(); virtual void onDetachedFromScene(); private: std::ostream& os; WorldItemPtr worldItem; osg::ref_ptr osgCollision; boost::signals::connection connectionWithSigCollisionsUpdated; void onCollisionsUpdated(); }; typedef osg::ref_ptr SceneWorldPtr; class SceneWorldManager { public: SceneWorldManager(); ~SceneWorldManager(); private: std::ostream& os; typedef std::map SceneWorldMap; SceneWorldMap sceneWorlds; SceneView* sceneView; ItemTreeView* itemTreeView; void onItemAdded(Item* item); void onWorldItemDetached(WorldItem* worldItem); void onWorldItemCheckToggled(WorldItem* worldItem, bool isChecked); void showSceneWorld(WorldItem* worldItem, bool show); }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/SimulationBar.cpp000066400000000000000000000051421207742442300227100ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "SimulationBar.h" #include "SimulatorItem.h" #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { Action* se3Check; } SimulationBar* SimulationBar::initialize(ExtensionManager* ext) { MenuManager& mm = ext->menuManager(); mm.setPath("/Options").setPath(N_("Simulation")); se3Check = mm.addCheckItem(_("Output all link positions")); se3Check->setChecked(false); return instance(); } SimulationBar* SimulationBar::instance() { static SimulationBar* instance = new SimulationBar(); return instance; } SimulationBar::SimulationBar() : ToolBar("SimulationBar"), startIcon(QIcon(":/Body/icons/startsimulation.png")), stopIcon(QIcon(":/Body/icons/stopsimulation.png")), os(MessageView::mainInstance()->cout()) { startStopButton = addButton(startIcon, _("Start simulation")); startStopButton->sigClicked().connect(bind(&SimulationBar::onStartStopButtonClicked, this)); isDoingSimulation = false; } SimulationBar::~SimulationBar() { } void SimulationBar::onStartStopButtonClicked() { if(isDoingSimulation){ stopSimulation(); } else { startSimulation(); } } void SimulationBar::startSimulation() { SimulatorItemPtr simulatorItem = ItemTreeView::mainInstance()->selectedItem(); simulationFinishedConnection.disconnect(); if(simulatorItem){ simulatorItem->setAllLinkPositionOutputMode(se3Check->isChecked()); simulationFinishedConnection = simulatorItem->sigSimulationFinished().connect( bind(&SimulationBar::stopSimulation, this)); isDoingSimulation = true; const static QString tip(_("Stop simulation")); startStopButton->setIcon(stopIcon); startStopButton->setToolTip(tip); simulatorItem->startSimulation(); } else { os << "Simulation cannot start. No simulation item is selected." << endl; } } void SimulationBar::stopSimulation() { simulationFinishedConnection.disconnect(); SimulatorItemPtr simulatorItem = ItemTreeView::mainInstance()->selectedItem(); if(simulatorItem){ if(simulatorItem->isSimulationRunning()){ simulatorItem->stopSimulation(); } } const static QString tip(_("Start simulation")); startStopButton->setIcon(startIcon); startStopButton->setToolTip(tip); isDoingSimulation = false; } choreonoid-1.1.0+dfsg/src/BodyPlugin/SimulationBar.h000066400000000000000000000014301207742442300223510ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_SIMULATION_BAR_H_INCLUDED #define CNOID_BODYPLUGIN_SIMULATION_BAR_H_INCLUDED #include #include namespace cnoid { class SimulationBar : public ToolBar { public: static SimulationBar* initialize(ExtensionManager* ext); static SimulationBar* instance(); virtual ~SimulationBar(); private: SimulationBar(); void onStartStopButtonClicked(); void startSimulation(); void stopSimulation(); std::ostream& os; bool isDoingSimulation; ToolButton* startStopButton; QIcon startIcon; QIcon stopIcon; boost::signals::connection simulationFinishedConnection; }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/SimulatorItem.cpp000066400000000000000000000107121207742442300227340ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "SimulatorItem.h" #include #include #include #include #include #include #if QT_VERSION >= 0x040700 #include #else #include typedef QTime QElapsedTimer; #endif #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class SimulatorItemImpl : QThread { public: SimulatorItemImpl(SimulatorItem* self); SimulatorItemImpl(SimulatorItem* self, const SimulatorItemImpl& org); SimulatorItem* self; ostream& os; bool isAllLinkPositionOutputMode; bool isDoingSimulationLoop; bool isBufsUpdatedEvenetPending; bool stopRequested; LazyCaller flushCaller; TimeBar* timeBar; int fillLevelId; QMutex mutex; double actualSimulationTime; boost::signal sigSimulationFinished_; bool startSimulation(); virtual void run(); void stopSimulation(); void onSimulationLoopStopped(); void onFlushingResultsRequested(); }; } SimulatorItem::SimulatorItem() { impl = new SimulatorItemImpl(this); } SimulatorItemImpl::SimulatorItemImpl(SimulatorItem* self) : self(self), os(MessageView::mainInstance()->cout()), flushCaller(bind(&SimulatorItemImpl::onFlushingResultsRequested, this)) { timeBar = TimeBar::instance(); isDoingSimulationLoop = false; isAllLinkPositionOutputMode = false; } SimulatorItem::SimulatorItem(const SimulatorItem& org) : Item(org) { impl = new SimulatorItemImpl(this); impl->isAllLinkPositionOutputMode = org.impl->isAllLinkPositionOutputMode; } SimulatorItem::~SimulatorItem() { delete impl; } void SimulatorItem::setAllLinkPositionOutputMode(bool on) { impl->isAllLinkPositionOutputMode = on; } bool SimulatorItem::isAllLinkPositionOutputMode() { return impl->isAllLinkPositionOutputMode; } bool SimulatorItem::startSimulation() { return impl->startSimulation(); } bool SimulatorItemImpl::startSimulation() { if(self->isSimulationRunning()){ // use a conditional variable here ? stopSimulation(); } bool result = self->doStartSimulation(); if(result){ isBufsUpdatedEvenetPending = false; isDoingSimulationLoop = true; stopRequested = false; fillLevelId = timeBar->startFillLevelUpdate(); if(!timeBar->isDoingPlayback()){ timeBar->setTime(0.0); timeBar->startPlayback(); } os << (format(_("Simulation by %1% has started.")) % self->name()) << endl; start(); } return result; } // Simulation loop void SimulatorItemImpl::run() { QElapsedTimer timer; timer.start(); while(true){ if(!self->doStepSimulation() || stopRequested){ break; } } actualSimulationTime = (timer.elapsed() / 1000.0); isDoingSimulationLoop = false; callLater(bind(&SimulatorItemImpl::onSimulationLoopStopped, this)); } void SimulatorItem::stopSimulation() { impl->stopSimulation(); } void SimulatorItemImpl::stopSimulation() { stopRequested = true; } void SimulatorItemImpl::onSimulationLoopStopped() { double fillLevel = self->doFlushResults(); timeBar->updateFillLevel(fillLevelId, fillLevel); double finishTime = self->doStopSimulation(); timeBar->stopFillLevelUpdate(fillLevelId); os << (format(_("Simulation by %1% has finished at %2% [s].\n")) % self->name() % finishTime); os << (format(_(" Actual elapsed time = %1% [s], actual / virtual = %2%.")) % actualSimulationTime % (actualSimulationTime / finishTime)) << endl; sigSimulationFinished_(); } bool SimulatorItem::isSimulationRunning() { return impl->isDoingSimulationLoop; } SignalProxy< boost::signal > SimulatorItem::sigSimulationFinished() { return impl->sigSimulationFinished_; } void SimulatorItem::lockResults() { impl->mutex.lock(); } void SimulatorItem::unlockResults() { impl->mutex.unlock(); } void SimulatorItem::requestToFlushResults() { impl->flushCaller.request(); } void SimulatorItemImpl::onFlushingResultsRequested() { double fillLevel = self->doFlushResults(); timeBar->updateFillLevel(fillLevelId, fillLevel); mutex.lock(); isBufsUpdatedEvenetPending = false; mutex.unlock(); } choreonoid-1.1.0+dfsg/src/BodyPlugin/SimulatorItem.h000066400000000000000000000030371207742442300224030ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_SIMULATOR_ITEM_H_INCLUDED #define CNOID_BODYPLUGIN_SIMULATOR_ITEM_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class SimulatorItemImpl; class CNOID_EXPORT SimulatorItem : public Item { public: SimulatorItem(); SimulatorItem(const SimulatorItem& org); virtual ~SimulatorItem(); void setAllLinkPositionOutputMode(bool on); bool isAllLinkPositionOutputMode(); bool startSimulation(); void stopSimulation(); bool isSimulationRunning(); SignalProxy< boost::signal > sigSimulationFinished(); protected: virtual QWidget* settingPanel() = 0; virtual bool doStartSimulation() = 0; virtual bool doStepSimulation() = 0; /** @return time at the last flush */ virtual double doFlushResults() = 0; /** @return finish time */ virtual double doStopSimulation() = 0; void lockResults(); void unlockResults(); /** This must be called within the critical section */ void requestToFlushResults(); private: SimulatorItemImpl* impl; friend class SimulatorView; friend class SimulatorItemImpl; }; typedef boost::intrusive_ptr SimulatorItemPtr; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/WorldItem.cpp000066400000000000000000000265661207742442300220620ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "WorldItem.h" #include "KinematicsBar.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace boost::lambda; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } namespace cnoid { class WorldItemImpl { public: WorldItemImpl(WorldItem* self); WorldItemImpl(WorldItem* self, const WorldItemImpl& org); ~WorldItemImpl(); WorldItem* self; ostream& os; ItemList bodyItems; signals::connection sigItemTreeChangedConnection; ConnectionSet sigKinematicStateChangedConnections; bool isCollisionDetectionEnabled; LazyCaller updateCollisionsCaller; KinematicsBar* kinematicsBar; struct BodyItemInfo { BodyItemInfo(int numLinks) : worldCollisionLinkBitSet(numLinks) { kinematicStateChanged = false; worldCollisionLinkSetChanged = false; numValidPairs = 0; } typedef dynamic_bitset<> updateFlag; bool kinematicStateChanged; bool worldCollisionLinkSetChanged; boost::dynamic_bitset<> worldCollisionLinkBitSet; int numValidPairs; }; typedef map BodyItemInfoMap; BodyItemInfoMap bodyItemInfoMap; boost::signal sigColdetPairsUpdated; boost::signal sigCollisionsUpdated; void init(); void enableCollisionDetection(bool on); void clearColdetLinkPairs(); void updateColdetLinkPairs(bool forceUpdate); void onBodyKinematicStateChanged(BodyItem* bodyItem); void updateCollisions(bool forceUpdate); }; } namespace { class ColdetLinkPairEx : public ColdetLinkPair { public: WorldItemImpl::BodyItemInfo* info[2]; ColdetLinkPairEx( WorldItemImpl::BodyItemInfo& p1, Link* link1, WorldItemImpl::BodyItemInfo& p2, Link* link2) : ColdetLinkPair(link1, link2) { info[0] = &p1; info[1] = &p2; } }; } void cnoid::initializeWorldItem(ExtensionManager& ext) { ext.itemManager().registerClass(N_("WorldItem")); ext.itemManager().addCreationPanel(); } WorldItem::WorldItem() { impl = new WorldItemImpl(this); } WorldItemImpl::WorldItemImpl(WorldItem* self) : self(self), os(MessageView::mainInstance()->cout()), updateCollisionsCaller(bind(&WorldItemImpl::updateCollisions, this, false), IDLE_PRIORITY_NORMAL) { isCollisionDetectionEnabled = false; init(); } WorldItem::WorldItem(const WorldItem& org) : Item(org) { impl = new WorldItemImpl(this, *org.impl); } WorldItemImpl::WorldItemImpl(WorldItem* self, const WorldItemImpl& org) : self(self), os(org.os), updateCollisionsCaller(bind(&WorldItemImpl::updateCollisions, this, false), IDLE_PRIORITY_NORMAL) { isCollisionDetectionEnabled = org.isCollisionDetectionEnabled; init(); } void WorldItemImpl::init() { kinematicsBar = KinematicsBar::instance(); } WorldItem::~WorldItem() { delete impl; } WorldItemImpl::~WorldItemImpl() { sigKinematicStateChangedConnections.disconnect(); sigItemTreeChangedConnection.disconnect(); } void WorldItem::enableCollisionDetection(bool on) { impl->enableCollisionDetection(on); } void WorldItemImpl::enableCollisionDetection(bool on) { if(TRACE_FUNCTIONS){ os << "WorldItemImpl::enableCollisionDetection(" << on << ")" << endl; } bool changed = false; if(isCollisionDetectionEnabled && !on){ clearColdetLinkPairs(); sigItemTreeChangedConnection.disconnect(); isCollisionDetectionEnabled = false; changed = true; } else if(!isCollisionDetectionEnabled && on){ isCollisionDetectionEnabled = true; updateColdetLinkPairs(true); sigItemTreeChangedConnection = RootItem::mainInstance()->sigTreeChanged().connect( bind(&WorldItemImpl::updateColdetLinkPairs, this, false)); changed = true; } if(changed){ sigColdetPairsUpdated(); self->notifyUpdate(); sigCollisionsUpdated(); } } bool WorldItem::isCollisionDetectionEnabled() { return impl->isCollisionDetectionEnabled; } void WorldItemImpl::clearColdetLinkPairs() { if(TRACE_FUNCTIONS){ os << "WorldItemImpl::clearColdetLinkPairs()" << endl; } self->coldetPairs.clear(); sigKinematicStateChangedConnections.disconnect(); bodyItemInfoMap.clear(); for(size_t i=0; i < bodyItems.size(); ++i){ BodyItem* item = bodyItems[i]; size_t n = item->body()->numLinks(); for(size_t j=0; j < n; ++j){ item->worldColdetPairsOfLink(j).clear(); } bodyItemInfoMap.insert(make_pair(item, BodyItemInfo(n))); } } void WorldItemImpl::updateColdetLinkPairs(bool forceUpdate) { if(TRACE_FUNCTIONS){ os << "WorldItemImpl::updateColdetLinkPairs()" << endl; } if(!forceUpdate){ ItemList prevBodyItems = bodyItems; bodyItems = self->getBodyItems(); if(bodyItems == prevBodyItems){ return; } } else { bodyItems = self->getBodyItems(); } clearColdetLinkPairs(); for(BodyItemInfoMap::iterator p1 = bodyItemInfoMap.begin(); p1 != bodyItemInfoMap.end(); ++p1){ BodyItem* bodyItem1 = p1->first; BodyItemInfo& info1 = p1->second; BodyPtr body1 = bodyItem1->body(); for(BodyItemInfoMap::iterator p2 = p1; p2 != bodyItemInfoMap.end(); ++p2){ BodyItem* bodyItem2 = p2->first; BodyItemInfo& info2 = p2->second; BodyPtr body2 = bodyItem2->body(); for(int l1 = 0; l1 < body1->numLinks(); ++l1){ int l2; if(p1 == p2){ continue; //l2 = l1 + 1; } else { l2 = 0; } Link* link1 = body1->link(l1); std::vector& pairs1 = bodyItem1->worldColdetPairsOfLink(l1); for( ; l2 < body2->numLinks(); ++l2){ Link* link2 = body2->link(l2); //if(link1 != link2){ if(link1->parent != link2 && link2->parent != link1){ std::vector& pairs2 = bodyItem2->worldColdetPairsOfLink(l2); ColdetLinkPairPtr linkPair = new ColdetLinkPairEx(info1, link1, info2, link2); self->coldetPairs.push_back(linkPair); pairs1.push_back(linkPair); pairs2.push_back(linkPair); info1.numValidPairs++; info2.numValidPairs++; } } } } } for(BodyItemInfoMap::iterator p = bodyItemInfoMap.begin(); p != bodyItemInfoMap.end(); ++p){ BodyItemInfo& info = p->second; if(info.numValidPairs > 0){ BodyItem* bodyItem = p->first; sigKinematicStateChangedConnections.add( bodyItem->sigKinematicStateChanged().connect( bind(&WorldItemImpl::onBodyKinematicStateChanged, this, bodyItem))); } } if(isCollisionDetectionEnabled){ updateCollisions(true); } } void WorldItemImpl::onBodyKinematicStateChanged(BodyItem* bodyItem) { if(TRACE_FUNCTIONS){ os << "WorldItemImpl::onBodyKinematicStateChanged()" << endl; } BodyItemInfoMap::iterator p = bodyItemInfoMap.find(bodyItem); if(p != bodyItemInfoMap.end()){ BodyItemInfo& info = p->second; info.kinematicStateChanged = true; updateCollisionsCaller.setPriority(kinematicsBar->collisionDetectionPriority()); updateCollisionsCaller.request(); } } void WorldItem::updateCollisions() { impl->updateCollisions(true); } void WorldItemImpl::updateCollisions(bool forceUpdate) { if(TRACE_FUNCTIONS){ os << "WorldItemImpl::updateCollisions()" << endl; } for(BodyItemInfoMap::iterator p = bodyItemInfoMap.begin(); p != bodyItemInfoMap.end(); ++p){ BodyItem* bodyItem = p->first; BodyItemInfo& info = p->second; bodyItem->updateColdetModelPositions(forceUpdate); info.worldCollisionLinkSetChanged = bodyItem->updateSelfCollisions(forceUpdate); if(forceUpdate){ info.kinematicStateChanged = true; } } for(size_t i=0; i < self->coldetPairs.size(); ++i){ ColdetLinkPairEx* linkPair = static_cast(self->coldetPairs[i].get()); BodyItemInfo* info1 = linkPair->info[0]; BodyItemInfo* info2 = linkPair->info[1]; if(info1->kinematicStateChanged || info2->kinematicStateChanged){ bool prevEmpty = linkPair->collisions().empty(); //linkPair->updatePositions(); bool empty = linkPair->detectCollisions().empty(); if(prevEmpty != empty){ info1->worldCollisionLinkSetChanged = true; info2->worldCollisionLinkSetChanged = true; } if(!empty){ info1->worldCollisionLinkBitSet.set(linkPair->link(0)->index); info2->worldCollisionLinkBitSet.set(linkPair->link(1)->index); } } } for(BodyItemInfoMap::iterator p = bodyItemInfoMap.begin(); p != bodyItemInfoMap.end(); ++p){ BodyItem* bodyItem = p->first; BodyItemInfo& info = p->second; info.kinematicStateChanged = false; bodyItem->worldCollisionLinkBitSet = (info.worldCollisionLinkBitSet | bodyItem->selfCollisionLinkBitSet); if(info.worldCollisionLinkSetChanged){ bodyItem->notifyWorldCollisionLinkSetChange(); } bodyItem->notifyWorldCollisionUpdate(); info.worldCollisionLinkSetChanged = false; info.worldCollisionLinkBitSet.reset(); } sigCollisionsUpdated(); } SignalProxy< boost::signal > WorldItem::sigColdetPairsUpdated() { return impl->sigColdetPairsUpdated; } SignalProxy< boost::signal > WorldItem::sigCollisionsUpdated() { return impl->sigCollisionsUpdated; } ItemPtr WorldItem::doDuplicate() const { return new WorldItem(*this); } void WorldItem::doPutProperties(PutPropertyFunction& putProperty) { putProperty(_("Collision detection"), isCollisionDetectionEnabled(), (bind(&WorldItem::enableCollisionDetection, this, _1), true)); } bool WorldItem::store(Archive& archive) { archive.write("collisionDetection", isCollisionDetectionEnabled()); return true; } bool WorldItem::restore(const Archive& archive) { if(archive.get("collisionDetection", false)){ archive.addPostProcess(bind(&WorldItemImpl::enableCollisionDetection, impl, true)); } return true; } choreonoid-1.1.0+dfsg/src/BodyPlugin/WorldItem.h000066400000000000000000000027651207742442300215220ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_WORLD_ITEM_H_INCLUDED #define CNOID_BODYPLUGIN_WORLD_ITEM_H_INCLUDED #include "BodyItem.h" #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class WorldItemImpl; /** WorldItem handles collisions between bodies in the world. On the other hand, self-collisions in a body are handled by BodyItems. */ class CNOID_EXPORT WorldItem : public Item { public: WorldItem(); WorldItem(const WorldItem& org); virtual ~WorldItem(); inline ItemList getBodyItems() const { return getSubItems(); } std::vector coldetPairs; void enableCollisionDetection(bool on); bool isCollisionDetectionEnabled(); void updateCollisions(); SignalProxy< boost::signal > sigColdetPairsUpdated(); SignalProxy< boost::signal > sigCollisionsUpdated(); protected: virtual ItemPtr doDuplicate() const; virtual void doPutProperties(PutPropertyFunction& putProperty); virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); private: WorldItemImpl* impl; }; typedef boost::intrusive_ptr WorldItemPtr; void initializeWorldItem(ExtensionManager& ext); } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/WorldView.cpp000066400000000000000000000107731207742442300220670ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "WorldView.h" #include "WorldItem.h" #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; const char* emptyNameString = "--------"; } namespace cnoid { class WorldViewImpl : public boost::signals::trackable { public: WorldViewImpl(WorldView* self); virtual ~WorldViewImpl(); WorldView* self; ostream& os; WorldItemPtr currentWorldItem; QLabel currentWorldLabel; CheckBox collisionDetectionCheck; signals::connection connectionOfCollisionDetectionToggled; signals::connection connectionOfCurrentWorldItemDetachedFromRoot; signals::connection connectionOfCurrentWordItemUpdated; void onItemTreeChanged(); void onItemSelectionChanged(const ItemList& worldItems); void setCurrentWorldItem(WorldItemPtr worldItem); void update(); void onWorldItemDetachedFromRoot(WorldItemPtr worldItem); void onCollisionDetectionToggled(bool checked); }; } WorldView::WorldView() { impl = new WorldViewImpl(this); } WorldViewImpl::WorldViewImpl(WorldView* self) : self(self), os(MessageView::mainInstance()->cout()) { self->setName(N_("World")); self->setDefaultLayoutArea(View::CENTER); QVBoxLayout* vbox = new QVBoxLayout(); QHBoxLayout* hbox = new QHBoxLayout(); hbox->addWidget(new QLabel(QString(_("World")) + ":")); currentWorldLabel.setText(emptyNameString); currentWorldLabel.setAlignment(Qt::AlignLeft | Qt::AlignVCenter); hbox->addWidget(¤tWorldLabel); vbox->addLayout(hbox); hbox = new QHBoxLayout(); collisionDetectionCheck.setText(_("Collision detection")); collisionDetectionCheck.setChecked(false); connectionOfCollisionDetectionToggled = collisionDetectionCheck.sigToggled().connect( bind(&WorldViewImpl::onCollisionDetectionToggled, this, _1)); hbox->addWidget(&collisionDetectionCheck); vbox->addLayout(hbox); vbox->addStretch(); self->setLayout(vbox); ItemTreeView::mainInstance()->sigSelectionChanged().connect( bind(&WorldViewImpl::onItemSelectionChanged, this, _1)); } WorldView::~WorldView() { delete impl; } WorldViewImpl::~WorldViewImpl() { } void WorldViewImpl::onItemTreeChanged() { } void WorldViewImpl::onItemSelectionChanged(const ItemList& worldItems) { if(TRACE_FUNCTIONS){ os << "WorldViewImpl::onItemSelectionChanged()" << endl; } WorldItemPtr worldItem = worldItems.toSingle(); if(worldItem && worldItem != currentWorldItem){ setCurrentWorldItem(worldItem); } } void WorldViewImpl::setCurrentWorldItem(WorldItemPtr worldItem) { if(TRACE_FUNCTIONS){ os << "WorldViewImpl::setCurrentWorldItem()" << endl; } connectionOfCurrentWorldItemDetachedFromRoot.disconnect(); connectionOfCurrentWordItemUpdated.disconnect(); currentWorldItem = worldItem; if(worldItem){ connectionOfCurrentWorldItemDetachedFromRoot = worldItem->sigDetachedFromRoot().connect( bind(&WorldViewImpl::onWorldItemDetachedFromRoot, this, worldItem)); connectionOfCurrentWordItemUpdated = worldItem->sigUpdated().connect(bind(&WorldViewImpl::update, this)); } update(); } void WorldViewImpl::update() { connectionOfCollisionDetectionToggled.block(); if(currentWorldItem){ currentWorldLabel.setText(currentWorldItem->name().c_str()); collisionDetectionCheck.setChecked(currentWorldItem->isCollisionDetectionEnabled()); } else { currentWorldLabel.setText(""); collisionDetectionCheck.setChecked(false); } connectionOfCollisionDetectionToggled.unblock(); } void WorldViewImpl::onWorldItemDetachedFromRoot(WorldItemPtr worldItem) { if(TRACE_FUNCTIONS){ os << "WorldViewImpl::onWorldItemDetachedFromRoot()" << endl; } if(currentWorldItem == worldItem){ setCurrentWorldItem(0); } } void WorldViewImpl::onCollisionDetectionToggled(bool checked) { if(TRACE_FUNCTIONS){ os << "WorldViewImpl::onCollisionDetectionToggled()" << endl; } if(currentWorldItem){ currentWorldItem->enableCollisionDetection(checked); } } choreonoid-1.1.0+dfsg/src/BodyPlugin/WorldView.h000066400000000000000000000006521207742442300215270ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_BODYPLUGIN_WORLD_VIEW_H_INCLUDED #define CNOID_BODYPLUGIN_WORLD_VIEW_H_INCLUDED #include namespace cnoid { class WorldViewImpl; class WorldView : public cnoid::View { public: WorldView(); virtual ~WorldView(); private: friend class WorldViewImpl; WorldViewImpl* impl; }; } #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/exportdecl.h000066400000000000000000000004541207742442300217560ustar00rootroot00000000000000 #ifdef CNOID_EXPORT #undef CNOID_EXPORT #endif #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) # ifdef CnoidBodyPlugin_EXPORTS # define CNOID_EXPORT __declspec(dllexport) # else # define CNOID_EXPORT __declspec(dllimport) # endif #else # define CNOID_EXPORT #endif choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/000077500000000000000000000000001207742442300205445ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/block.png000066400000000000000000000020201207742442300223360ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<IDATH‰½•{lTEƳ÷n÷º»·«ŠF$--  ÏHP±ÝÐÔmŠAHhjc¢–4‘DRÔ ƒš DÅÚ ‘¢ÆÔøhbÚÊÚ‡ Q±…j­5E·îvûÚW»ãÛm—ö.¥è—œäΙs¾oæÜ™9BJI*¼,D¥„Óý&h{BÊ’”‰IP/7) ¯®ËMò…jp^ ù¬`ŸKÂ4˜æ?gL.Hl##ÝÆAÅDfÂWº¨p)=M3’"iéé;h›|tŒ]*€(G·[ñlYEn^.æÉ€ïtè™™¨iªúb ·¦">qêÚÁšÆfU<Œ¦ë¸×çs[­Ë|±µ Õœ€"ƒ-kÑý. €ñ±(¶²XÛØ@Ç_ÐðlZîTÈÇ5•×lļþ3Ü÷‰‹!?·Ç¢³U€s>´88z÷zo,À? ûË Ë û¾f¿ XXG¬Ò…+ Wž¡åþB|=¿qWp‘‚ÜcNãÔ¼L6~äfkö’I¿³ –-ˆ €Á)²Ì_€³áÞü;©³ë„§ÍG­6Î._CûGìIäF0<¦ŠEcÝO±>òoÛu&ü£ÀA»NpÓVòê=˜Ó¯6$õCí70.Q†•¸ù©XòÒ[ÔÚu~jì:™;w±úÕ5õ•Ûþ‚ ÆT@«ÿq²ÌŠQx)×”-æŸúmœ.~¯Rï¤ä&‡Êï€wÛÀ„$d2!¥€HÌØ̽X½Ø¼)cPó 8®Š«ؼYéš¹‹qn÷“ø¾¬¡$à«ægâ«öÕ¡X4Ã8« Ã:5NùE‡´Þ»óájʇÌʆYÔøÍ…+ù£;u€D0ìü™ãËY}º-ÃC$ÿg8Dqw-Å·pÑÝ`HÞ€ê#1Íøóóù¶$Ÿ{úzYäHÉC>Îo/å—=OO-w¾O8åÙèë´*dåõvg]Kø¦¦ç-YÏië¥D'~v×ÀÔë7³ÀñKç&üÙùcÍÇFNþŽnQÙ;¼—»ÞìäB_¡(Ë" ‹—vNeÿݾ!#ì[œ;MàˆP ‹'Æ#j¦¿õW<HüÑ78>,¥LiUðÁùx.±=à»\^²ýçíÿk™FˆAÿg0h†Kšƒ€À• ü öõu›‚Щ½IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/center-cm.png000066400000000000000000000015761207742442300231400ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ûIDATH‰•MHÔQÅ÷?£“Z–9B} Tf5b-ŠjIùf.ÚE­,£ L¥(-jQaôMPSA_PADۂĈ¦…R6¥ˆY6.&srn‹™! çÿ×¼Í{÷ÜsÎ垨*N«Y$+Ïãiž–›[$™™V8é|ßÓSS§úÝ +N"9…–õtãªU>Oi)Uåqkk0 n؉„ìðn'³àܦü|Ÿ» fÏŸ´ìlʳ²F8 l´Ã[Nù°À-òç@b1D„^oQƒˆ­HÛËk5d00}}ÐÕiižŸ>19ÍðÀ`à¿êTc÷EzšOG¸\q"— ¾~¥7êݧš²¹#@î³í°riw·E8 ^/¸\|èÐPxðªUµÝà¯^Ë2½‹;ÔC!~€»»œbSí„·}¦"[s Ú ¼¨çÖæ ð xÜðræ°˜¯H=&{õ¦Ì/زhŒ»0#`šm{¤n¾e.˜ŸPÙš<»U—¡bÉE0C`æ¤êc“W#0 ràŒˆo1*†“çE|‰¢Z`82¡)£à¯UUêaêhÓxÌô¼9‰ÚC`b`VLÄA3Ð?ZD¬…pc,K^®âyp§A$húg`*ãê+·©*×àÔ üJªOîaЛð ,0ÛãSaë@ÄŸ4¯¡øÊi‘õ“ "_Áçd]'|iƒ÷Ó¡$/^x4‰øÿï¿¡JÌ~ý¿J.AcRýc¸5†óò„‹c:ˆ‡JkGª7Ÿ¦|)–jà!ð¨ñOIžÑp- ±=c5pþú ðûþ"©œ ²¸ zûm*‘NÝU¯€ë@µˆæ(ÉPQ— `P),[t  øŒ€uzQ DLP´W€&‘?«³ÙÌ@Ät«uïx?ðBÕ>²ÐÙˆ˜ÀNàœªWí½ 1åÿm@òÞ÷7g¶ :€! õ¿ DL=°ƒ$ýäÔ~2—C"Ámÿl"üÜú³ë~€ÕúO"¦¨ÚUíÄì¾êã1K@£ˆÙþ×$é?wæ¾D;€ïÌ3‹Œ"æPœÍD?-¯v¸ Vfe b$Eôèš~¦:osÍ"S‚Ã@9pFÕvRO¦‹@½HhǼiô}À½…Ägª ·m^ ”‘%ýT©Þþ \ˆwf4HÑŸÞ÷³†Ÿ.ÏUàëìY¤'8 lÚTm7“„¯É'sÉ'SÈ`¿H¨z\U1ðP ,ÝÀ×äg¹ÓbXboñ2xHDC±hìîÀ+é&"Eàùô¨ÚûrS=lÌ,ñ§ÄyÙÙè¯òUUã÷ø)É)á鋧Ö_Œ|Ô‡…‡“)žŒ‹˜óÀ%S£j¿²Rô§R ¦9+Ãdž·k™-ÍYŠ/ÇÇúüõì]²—Àîx]MÞʼÙOú5 Lò-Œ„€@«ª=ýõ) •6D+£Î*‡¸Æ™p'uG 'ÂôLô0è ’¿&_ Ë kKMé®™YØQóÀ>‘æÚÜ}/ð(ÃõºÍ±­±eSëÉVÄVÐÞ/CÎ}±>Æ*Æ–»¯]¼Lùuà$hk.à-éô2!›‹ŸžZ0B·ÕM¯ôb‰…£ãî8qK^<ï·¿ U;*Ò|Ôüh/!Þ©{IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/cm-to-zmp.png000066400000000000000000000021311207742442300230720ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ÖIDATH‰¥“]L›uÆÿ¶@ùœŒ–44Œ„1ÜpcqÃD‡‰ƒ8³DÅh|‹5#õÆ#!nQ'Sш&1a62“Ũ˼1!ÁhYê4°•ÁøpØRFß¾íñ¢Ô´8Zu'yoþç9ç9ÏyΫD„Lñ†R»œ……ÝÅÇ–XNNx~iéW¿ßÿôa‘«™jU&‚w”j¬ÍÏÿh_c£“½{¡ªŠˆ®óeÿùsçÎÝõ’Èéê-™&pÀñ}‡› ** ®Ž,à^ÃØJDPÊu;0 ñv&—”¶–6GJ"öûÖúíõ¦ÊX%W~»²zö‹³¾À\ ÕÒ?šŒWªy¤ ÌÛD/'<èf€×“Á%î’{®í¸öæRËÒ¶òšrSuQ5œhkh³¶¿Ð¾gSå¦!g“Ó–ªÂt0C¬ À¤”ëàεéWþžÄ£LÓ·|pÙ`ˆA8& ²]ÄÈ2¨q×l–G?LõtpTÈ!¥Ùm^|ÀÉ@ÇŽû¥‰;›ŠLá û00°*+ÓÆ4~«ŸÕ¢Õ›×{Ò´Bì5Zb÷ß&Cì.ûQGÔ+Œ­&Þ¬ÊJ®)…BG' a™¶ä™—ÍwÌ~:û{ª®‡Aú eRwÎIEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/collisionoutline.png000066400000000000000000000015111207742442300246430ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ÆIDATH‰µ–_HSaÆßvÎÜ2Ô™aA‰*DF°þ0²(„.&Qt•I ÕEXAň¥"»(Jý¤E( e¨”•b+ [‘8·Ù¶sºX;mmk çê}žïåyÎËù¾ï¡ª*ÿR¤°YE!àˆÖ²ÄV‡Sý4<Êú«³w>s z1Dk€Í*-)çÀª5 Dö:]¼™àôŽ½ÈB@P‘x>RCHÕ§eZœ7Nyñû¤ë’Í*V._‰­¢òwRç º>42ßJi>ú]¦ï‹à›}v´\k³ŠZ€^}ûî0›œGÇËnìÊÃÆ{®x5¾v,­æZ„÷<©r5ó†…#wÚ¨«*äëTjóWŸÁœ æY>M«±Äö|IþýÎ ÜëßHU‰ŽîáÔïÆ¡}'4ÝüðhºQöc”ý׶©¢êX`žâòŽ9©ÝƘô?Ï:{ø‰<32שâø€l`Í›ôXÊŠhyÎw91= vnúÈÍKY¸—ŠfOÇô$ ¤€õ<”¿TˆÜ^¹Ø|1\»§¡uœxÛž4 ¨@HUxv(½!;`n^¼®èu!Ü™ý·&P 0-óA õ†N–â ˜˜ äP{á5d¢ª`2üLoš$ß4 €?`D'2ûVÜí bYÒ©qÏH®ah=û§©¤êê3òÕpÀËÛ–&F>À@/$÷]µÃÒjúëêA·ÇH[÷t2¿¸¾É4o:NÁ¬ð=wý ŠªrDr8Uí®µY…1RçH3ÜÙW‹ÛS”VÀ–j/Ö '½/`°Ÿv‡S}óÏ“¼zqOZæÑP¸y/p œ¿>ÌØWCßKýÄY‡SOâÒá†ì€p.BÄÿþmùæ&ßq²ùŸIEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/fk.png000066400000000000000000000026601207742442300216560ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<-IDATH‰•–ilTUÇw:0˜¥3uÊÚ%eJ)„”é2ÐbK%Åcš41hIèP±jbŒŸŒ( ‰áh iCLc Õˆ42U´…HiÚJ§¥ ít™yïøa:e¨uá$79ï{ÿ¿sϹïæ)áÿÚÇJmLÛE>›,¾fÍš* xdd$0::zôÂ… Cê¿o+e© Uê[;ÌúV¿.Ò‘<¯ººú™… îq»Ý7ûúúº­ƒƒƒ/M 8 Ôöá ¤Ïs‚ГK‹Àñ9\üª‰¸\.Syy¹wýúõA¥Ô¹ÒÞÞîohhp'dk˓òLx )4XœxxJ¾„O€Í¹¹¹[–/_>¸."!¥TÐétÞÔ4mî]€Rj¯…U&à*ÜùÚR ’öÅ? š)xú RM[Eêm6ÛÆ9sæô*¥‚"B§OŸÎÖuýCBÿ#Ø\& ®EÊjDVwà ˜s#ÀT0Fá' W¸\®Y@(ytvv·¶¶žßžž©Ý0ú'Ø)r&3Á 7 ÷¸u:ಠàrJNÎñ¢¢¢„°ˆ„¼^¯1v‰ˆŒÌp~Ùû’ú’j‚÷ÎBsÈŸÉ=[±b…y‘ÓY`4{â@¨©©é]×÷Œ40@Œ¤£U'2 c³Ùl;=OjB\)ŒF£ý‘HäÖÖÖ¾1½¸Ž¦BÚûJY?Pêù}J˜L¥”Õj}Ìétö!ƒÁ‘ÐáÇ u]ÿ*1m|C0 Ó৅uNNÔݧÔÆ4£qGꌙëΟÏ·´ HEE­¦&„ü~ÿ£^¯÷ð8àC¥ÞtÂúB˜P¶°ýáøY·C))Ë32j‹òóÓ°Xâ/Ãá´–¯¿®jknžr婧:b±Øµä5ÆA8¤Á“y`O\» ))óueŠJKÓÈ·D Àc6O“,ÿ¾¡aÀèt¾˜¬cxMÄwV~çûAˆAø5¹Ùf£qGñ‚²² ?–.·;îge±,/ϺÄç+½xñbø@È@T€Æ[0z û&”'uÊ”LÌf°ÛaölX¼-Šûv;˜ÍdˆŒLìÙx“Ç.®šO•: CÕnhVJ©•+W®KOOßú„Í6ˆ—Eb±»þØ0êúÐ?¶YdÛ.›­xmYÙÞgíöj·ÛíÌËË ÛÚ¥¿ß¤ü~èì] wwƒß~û6ƒ±Xï¿<OÞÌ™3÷ά­]R[V6ÝápôÆûM0eíÚß;V]a±Ì@|¾ø¢@::hnoïñkÚ[JDðx<9™™™‡rss³KJJFM&“_DJ©`¼ß€`ßž=U9>_åÃóç;”Õ "èá0ÍW¯vÿÖßp‹È»”””Ìs¹\'kkk ƒÁ×®R* "ãþ¹sç¦u:õ\Qwwù,M2BôN4ê Š¼±[Ä;QÀhµZk*++á+!¤ëzpìÓ*¥GŽYÐÑѱJӴ߇M¦—G"“‰M ˆÅbÑHä:ð»R*‘} §§g¨¾¾¾";Eä¯×ûªÜÏB¢D¥¥¥‹Å²Ó¦M'ìv{wSSÓô–––UÃÃÃQ¥ÔÑK—.]¿_Ñ{"‚Ûí~Ðd2½¢iÚ¨¦i]###õmmmÿ» ÿfš¬]Nö§IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/fkik.png000066400000000000000000000014231207742442300221760ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<IDATH‰½”MHUA†Ÿ™kQ]ô¦"FéB¨¨Î"haTP í ¢ î"* *2‚+DAtZd‹¢mD‚J(Û„"…C?D¨‰–ÚÅòçN‹{ÆŽI‚xêƒácß¼ÏÌ|s^á8ñ^à-×Ú{O¡”[ÜÖIà!°¸†¸·€mÀ#aŒA)·8,ÑÚ닲RîB 4jííþ÷?¯œ‹¸Ëa5-Àî:/€Õè Fý¦¸‚Xÿ›V>û˜âkÿíÖÃBX¼  ÈÏCX"á8ñ`70híMÌ• ”Û $°ø@Ú æ,îÇ>Ò¦·ëÑâûU’ë7oIEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/flip.png000066400000000000000000000027311207742442300222070ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<VIDATH‰¥UyluþÞof»Ý£[Ú"= bA)Zi$ E+ ¢FŒ*Mˆš£&HEÔD„5¦ E‡PcD-A©àTAÑ”¶ÐnÓƒmwg;÷ÌóhÓB—|™dòÞ÷½cò 13† "=|áº< ÌŸè+]ÃŒwÔ-p?€ï¼I#0Àñ™3µ âºÅ0(©ë¤hšH* ”óçåÛM“̸ãRÍÃ>ôùø˜®Ó${å!ÙĬYzwa¡ÕÄŒ6!8Æ,bDh8öè£ÃC¦I¹—ÈÃDX“í¨®¾ðrUUtþ¹sò³âZÿ"V ô%K’{dõôˆéD8uÍ þI¡À3ÚWãÆYÞƒ³“IQ`æ \J¬Ð  ®‚V~3"-f1ìý^Çãºá8d³ËÎÎô¼:§òì/â‘x4êÞP]ݵCQ„¶xqæ†Aµ®‹ää¾ ¹fdxH¾YìC³5EŽÇExˆ¾‘-êñ ·ª(ß>‰â§–~/I¾aCð&à4f,€+zI–a¼ùô¾Å½ë+ß½ÉüñÞ6»™ÆÔ1F»eA·,ÒL³й=2í±’¼ô5èÞY;ñ7Ú–vôhöxf¼ÈŒÖ~"ŒðdŸo?w7Ü¿iFNNFY$ˆb1Ý2¬»|qϯu¡ÃuÑ P‡è<»PËÔ,«ôñòin |¹vFñõôQsW¬ëëºo‚eEòA㮳shŒol @Ð4$£öÕŽë¦ùÚ{TP6i< ó)X37Ër¬*@_sq‘àÑ·‰M ÷ù^àÇo åx<€¦š†?»âø4w-fתr ÔöeYJä*Òˆ)2@’Yˆ®¶9蚆æ3gL³·÷=Q‡çÎüÐ;•?«ÿmhSú±kË–”eš«—3'ló·™›ë7 :Þ$rJQÐfš°†0DÝqКJá§ó-°§ÿýѺ:«õܹ_–êúk 3ã<0ì–W©¡àËÓö‰œÉ“3FÊ2’–…ÝÝ6¤h—kë9;'‘J¡7•òYö¨Rž!ѬáÙ…Ñ(ÚÚÛ±ÿÀÓ1éÞ~3gæ~¼”¯öx’©’ŽËo§‡›jj:Ê ­uvj» È ¡%`.*2–¯uã'?—–²±p!¿ʶÇà+—ˆ<–™1ÈMW0VœÀë›oaŲ ÷:râqq'“T·Œ¹usk¹qõiO7 ÌÛ£ªNÍ7GLÿ„)'æ=_Z‰¸ÅD8I„‰WØõJ·§ã¨v'틵1edkÇ{'&âfflêËa˜kÛøcóœÙ?64‰Æ„®Œ\²þdY™¡VWwŠD\¨ºB€‹Žå³‚ 9oœV[ë›A„€½ó˜Ñ`º kEÜamâ¿æ³Ð¡x\ÊH¥HÐ8èß0WV*'vïhÙ³sgçÇ~¿«üîÀ¼ËðuD¼`=ÚJfd8º$qÀáË8¹—gû…Ç“sè 7 i”`óßéàiÚÌ'ÂÜæfy‘pÏ1#qy'AIâŸÏ5Ë˵d4ꨒÄ?þ]÷×ÂPãfü†,óiIâƒü¿4Ì3dòú÷IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/icons.svg000066400000000000000000005262001207742442300224050ustar00rootroot00000000000000 Icons of Choreonoid BodyPlugin image/svg+xml Icons of Choreonoid BodyPlugin Shin'ichiro Nakaoka S ? choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/ik.png000066400000000000000000000024151207742442300216570ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ŠIDATH‰¥”{LtÇ?÷^Š¨×pY>MD)I¦)+&¸ZÌç˜âdlâ#³ÍL4s!mþé\[«œKÚJ§‘ˆ¨€4pM–as"rA"Ïë…o͘ ÕÙÎ?¿óøü~çœß¡½ºZ¿oÙ¢Šðp•®Z¥ÆâbI:!)XÿWMkº»? zü˜ðÅGŽPâîäãã³ÑÃÃc90ð@ ÿR\ 7n@t4ÄÄÀŋ̶٘`6“““CSSSp@@ÀÁ77^š?_îõ@Í°øùÝPUE½Á€É×—…3fà¼~ÖƒÓÛË•G –˜˜ÅþÛ·§‹† 0Ôeg«å‹/°öôp£µ›Å‚WL þþþpàó&O†ÐP¨ª¢âÌ‚òò0¹»On ‹ éI=u……röô¨»»Û‘™™©äädK‡IHiij]¼X5ùù’´z¸M6H˜„•@5° ØcÛ¸qbˆÓ ³gCm-WÊÊhJL$22òCàóa•¨ð,YÑYW—v~Ý:&µµÑgµâµv-ª«Ù¶m›˜Â“ɲDÏÓeÍÍÍ}III*++SuuµJJJTXX(Ië‡S"ãüô±cǦ{zzÒÒÒBWW3gΤ´´à#2~h`ÿš5k(//§Éf£rÇ^niáŠÍ6 X1Tðózð´”½dIÈòŽ,t×Õq®¾ž•EEWWÿË >Õý>Óç:X-‚uëÅ\77UU½,¦g1C>©ÊÎþøÄÂ…f è&£\\ÀhD½½ÈépÞº¼wïÔ¼ ¢ú!#ÿ °µöòå}¶øxÜ::Lýç_µÏšES^¤¦â8ž«½½xNj cwC§x×®0àà2ÛxíÚá¢èh–ÖÖb4B b|\?Q\_ÏÍÐPZâânŸ¼ðZ{;:r[JÊ;@*`hòÒÖ;wÒ²ÂÃMËïÞÅÈ´ZéöòÀ+$kb"EEE,X°ooo222˜çêÊoûöáêêJO[ËîÝÃȵXðÞ¿Ÿ øø/]€×ÛëêN^ˆŒ4-íOe·?Ù²@úèÑ ž6ƒÁ@ke%a·ná7¨Î œÛ½w/¯ÍFàÝœØX·7nßæ…gt|pÒj¼ß¾ŸŸ7mÂü°èèQç/“&Ñõœ€aþ—¿$ÇbáÍÇqrÇLœ»8;ûûôˆãŠ{÷pNûúÒk6`þÀ`00>(ˆâÀ@L..t47³²¦†Q@®ÙŒ59™é«W÷ôbZßPQÑwÜ×WÐé9s$龤tIßvvv:vîÜ©ÔÔT;vLYYY’tIR–¤›ù jŒ§ò”I:%É4xû½o¿tI'¼½õcp°úƒl›G»Ýn×Ç%)G’g¿íƒü„eŒ¥’={$é¬$WIÏ\×»ïæåédx¸$d!iŠ$¿Aç[~MJRþÖ­’”Ûï‡$þ ¥ü¢M IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/initialpose.png000066400000000000000000000014341207742442300235740ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<™IDATH‰í“ßKSaÆŸ÷xÎæ6Ïü±9ÙÌ%”NWÞXÚ®£®Â? yD‚!ÔEcI”y‘ÙEިЅ ÝD$åEBPJxôÂ1´Æ~yÎÙ{Þ.jcÍ]ÑMàsu¾ïûyÞçýÞ/aŒ¡ÙlCÇL&˨ª¦¿„Ã}ç‹2àŠM&ËÞÞ3m­­Gº<ñÏ8Ž+7xØíÖ2@«.ÖÇg> Z¸ô@§³Ê˜S"$ؤƒî0²ç2Æ`4×ÖÖÌ44¸Ìz¥¥†Òövu{;Æ¥M=.•’µååõ¡Ð•³ÙDÑz±§çT}U•¨çËÊfIGG‹c/fjjŽrÛÆصm!FAà‹¥0>þZ2øïù&Ž#5~§se%¬ÍÎ.|6%Ÿ±ÛEWww›CJ Ù2J§)’Iåùêê¥þ|³Ûýè g2)Óµµ­~ƳùŒÇóä€ßº+úý­ö_h‘ÊÊ{F£Ð‘¹¥ôèÏ=ÂUTX·} iZ*—o%öÐR²¬BÓ(ÕRfóP—Ï×rÃçkÞ5x^o]‰×[w2S+Jcc¯š4Z)°lÀÖVldrr® Db£v»x¡±ÑeÖ4I ë¶_Vf‚ÃQQ4[ I|Ÿ_ª—¤oŒ]g ,lœŒ±€Z]}ÿ×íTLL̽Su¤P€Ûmôû;gêÍÍè¹éé…Ó”òoó~ÀX`×d€Å"HÑèÕg…ö<žÇ7skÆi/s×þÿgz°¯ NòÎNz=M¦ !<¥,¤gVU*€,+êÄã©ñ™™÷=„À¼±½«gN$äÃÃ/.Sª=Õc~Kö™7ÜfýIEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/left-to-right.png000066400000000000000000000020541207742442300237400ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<©IDATH‰­–oHÝeÇ?çùý¼WÓ{».»W·Q-W6r¸åšƒÑlFä†dÁ`…‰RÐ^íHPË›Œö*ˆÝŠµ‰o¦/ZAT(,+r9Û*š^£¸Ó™]õ^¯ÞßéÅîÄô**xÞ<ßóœÏÃyÎïËUe¥ ´Ð Y–[–Û¶þúòrglV;wNDz²œË¥^·ÏåË®¼PÈ®>Zê̪õõãª|¯Jž뇇ïÎ …–/a–NŠl8!ÒÙµšK,Œ”ø ˆß‚¯ÖÿWÀ)‘l ¾|Â#0û×<íàAß½ÆØŒÁ2kbBÜ€[„]@ð£*ç—¼+â™…/„¿«à‡àéùúÄ„Ë'Â^Už1†›Žƒ_„‡TygN¿=~pZ$c.m©¯o ìùÒ Œ HBÒӧܛòEÄ„r«£ßÝuØêïOËr»uèܹѪcDzܸa½«r’qMBG˜jèºC®ì‡ð~°+Áó"ñìWt½ñëv±ùXËË£cŽ#ÞH„¼††±ÎÌL ‹è,± ´{ÁS môÀ±-.„[À?›E¢Xi ¿È\ÀÌ ž¦&_­×«¾ººHHpÀvCÁ«ÐœNŠAXI|~õª]ÑÓãÚ²{w,VZ:Ý/¢"l6.NƒïxlÅáö›ýÔÚê©L$ŒÿСÈ`v¶ÆEh2oªÏöÀK=ððŽ*5££&çìÙÌ'ÒÓuÅÅÓqc(™›¢F‘'‹оÆï(½§ª8è©Žwf´ð†*§D8i õEEñh_Ÿ+}v–æÿ8_žj„ÑhUxï}¸Öæ¹i¨.XheR·AlÛému/²× ™¿¯J?÷öºöŸO×Σu"‹ë- HB¾}[5¶p_•æ¡!{ëȈ}¿ŠŠÉaÇ‘@ÙªËćªÐÕ•Q""¹Û·Ï¿ß7fq›ÖPeèèîv8 ››0Æè¦É+ÿ«X0®e ZR2=žŸŸJ~¯/Ê[+ 9jYú‹eé%Ð}©rþöT<ì×@ÌIEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/left-zmp.png000066400000000000000000000014051207742442300230100ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<‚IDATH‰­•ÍKTQÀç='ɉ1&Ÿ† AµqÑÖ"A¥¨…¥¡£“ZÔ¾A›6-ú ‚VA ‘g¡Ð.[è¶)SDBËñsxã|Î@è¼™'u–÷žs~÷þî»÷‰ªâ5Db@HÕîô\ã Ò€š-ÀUíU/u†×•@MPõZu€DßÀw`Às•E"]ÕPû °ŸÀà´Mw¼ Aß 5yUÔlÂÉIU;Î4Uˆ<ðöïT_f Ãc@»HÌúgl^B=Å+ÔöýýÀ.Ô~.¨Ú_ñ¨©,@$f‚Þ>¨¾J˜:*iª°iNÐó7 ¢¦ €|pÀÿñàLAÓ4¹DD@ú€Oª¯“.iů©þȈ]]ôÃfÿñs½teù(yï–áES€DIU{Ë=¨ ©$@dð2Ð ú¦Bó"ÀÄÐ’šJ¾¦"ÏA–ªpëÜ4ÔÔãú“9Þš­Iç|â[6•2æÃÕñU§ öXRµo”jlõZF ‰\‰\ ;â0“šaÚ™fjn*ç÷/˜fÿÚèZü"‘Á‹À¹rzÌ:ói¤;Ò=|w8Üì ¥º…†ªBf³ßܸ¿q!ÊZ½Vu‰3ÈE<ø&\Voõƽpk8°ž[gvo–•Ì Ûùmœ¼Ãžî¡Ç”Û;çsu¹gU‡[È-à‹êHÉ¿•Vi[òRÒšOÏãË™eRšb1³ÈRf‰d~ÿNfÎf|zL;KxÄÝô¨_Û˜&ÇÓ Æ>ñ¡ª8Ù=_r7k„ÖÄÔ4€±mœùÄÈù¯I_ø»IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/origin.png000066400000000000000000000012361207742442300225430ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<IDATH‰µ–MkQ†Ÿ3MH›±!†B[È¢fQÚ•4‘Á­øU’Øà>Õ… d!Ò…nÄE‹váG ®W~A¥(Ô`ÁR(-4m¨é¤‰™7’J¦±N_8›;œç=gî=—+ªJ»IäTçδ›c´M? \7IÞI^=‘dt ¹nðžɪæîºj°>Ùøí¿ œàÒÎ1½/20Í”u¤wÔöxÕ¬ç‹[[éëå q'x[Ó"¢==ã'b±£ B8Œz½¼~þšùRô¿´"-á'ø‘ä˜a\ŠF"Az{a`††@€sýý~û^f;öíÕ'†ãtC*êóñù « º»¡¯†‡!âüå‹f¨³óŽÃSÿ½`ÿq~U¡Rr™gï¾sÚ \*A¡€aYlyB§Z1`Ÿ_@­…“oÖÉ.nðÔ¶™ Ãæ&,/Cµâ˜îqº¸fE>''ÉÚ©c5&d¾®Âö6»++¬µªsgÔÁOÈ^ad6gG|)c‘Œ½ Aèè€Þ/-­nT«iÇTµe@"q3²£ õØ} kÓpÛ)_U[wÐ8¡1><†”…9j#jRš/Búšê'Çê[uP¯™æõxâùýªnŒ¿æ`¿»å_Õdà6¼É@$yÃmx“°rËM84Ìjî¡›àºýUñ¬’>´×c¾IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/restorepose.png000066400000000000000000000023611207742442300236260ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<nIDATH‰­–[lTuÆsÎÙÝv»íS@S(5•€U¹ØXÔ@ rñ¡¡ÕÄp !Š>Tc}C#‘Ð(BBDBlú ^€”H” †˜’H eÛÒRÚíÞÎîž3>´[–v‹’8ɼ|sf¾ÿÌ|玨*¹L„`^Î DTIŒ¿Sg<¿_;*+S…nŸmáx\±˜Æbf¸³Ó\¬J­*'³´˜ T%™Á­{±/]»]Ue·‚´WE´¸ZW7©JU2… Ã`aèK a×¥xù?ü›‰à5MýÒ륺¾>ÒpåŠD ìÁ›é•PÌÎçç#ÇŽŸ9“73—RÛæ±hÔ G"V• `Yœ(*ÒŠÆÆþ}"Z°k×ïˆè!ו‘¡ª íAƒ"î_†¡×A£"šÕñܲ4Q^žênn¾ùÅŽ}»§Ã4ÝC ¾LMU½Óa°½°Pöîí}sõê’æD‚Š•+cí©ɤÑ‹ÑËÀà  tvZù]]æÚ5k"‰žkJccñ’DB¿vyM7{*Öðh›&¯44 lnmµ·m™"‚[WéQ•aRÕªFíÛ—_xüx`mSSÑädRt]ùØqäÝ\{²D(2Mškj¢¿ÌšåøÖ­+®á"hUÎÅ:¶Lûcýӕ锦ýj2ÛÊ9ym“ð*`(˜€ÜÜ zÀ2 Þ›6--«VEûZZ oݲ‚ŽÃÓ$'ÁõæsË®}VßÀ~ò‰Õš8®€CŽò'xm¸°¼^]Q‘ò\ºä™~䈆ãð"P –ÏqÄŸNKa2)Ŷ-ÉdO°¤ÓYÍ €ôNƒcCwžËè¨Ê²ô6¨kYºiXQ¯§žÑÝsÏ@z „aʈŠT9/B P’JÑ=¬¨'=½ìóÑ˳ÀàœoùÍÈ“.AǾ¤§ ‚76¨vŒ¨@Gu¨¸EªºbùòèÎææ[›'LÐð3èü•¿Í3v®ýÔÏ Ÿn™pÁ([ j¨¦&–ŠDäápXžNdðGÊ#®1’ÍJœÌbàóq <­_´(þ·ˆ–>œ·D„›ªœÎàfAáDŲ΃³ 4”•ÿxx¾I$8†@„jÊ–-‹%@¦ž=›WN³'7<ù%®kš=½Àæ£ÉÔ0‡ÀN Ýic:(óûÕq¦¶µ™3»ºÌR %ïJ»Ô6¾‡þ$Ì{6öBßå»GðÂàÎe7,O^ž{ÊãQÛçsn?áù&ù>þx#ÌÌäm…o/dÉõh´è¢ct.è|PÉ…7²ÀÊ`[`â‡îÎ"øìðö]·i¶©ráÞø#XÖÌߤ¬ç:!®p9×îÛl`äÃÞtAøé!PØß éßA;€¯ æ@íÕ0Üã¯â~쑧¼ð‰ÀÔ4ì\¯º-ûòk‘MÎ IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/right-to-left.png000066400000000000000000000020701207742442300237360ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<µIDATH‰­–mh•eÇ×ýœ³·v˜[ÓÓ÷‹°Æš µ•’ƒ%Ë2úH‹^'Ë>¸Åih°ˆ¾H¶•BHQd„¤YiLY bÒœ”N˜›s.çvvÎÙ9繯>¸åÙÙ›®.¸¾ÜÏÿþÿ¯ë¾¯çϪ2W‚–;Žö:Ž½ê8ÚgŒís­oOrz˜?ªÓÒ4¯º:4Êh4*cæ&¤.°íN,$@F†×ÔŒ_ép]÷äýóE³H®'¶½«úÛló_€L…Žìyl6€Ü¾Ì„a=° øXc ïdg»!×ŵÖÄ]×6ƒÎþÉ ÓàÌÓðûOPâÂÚ€ê…D>OùsÀAc²–·E4l­˜áa'óÊIO®²®D ~~Ù#RѤzyF"d‹Ð½d‰^nk»^¿}û²=##RUPàFÊ|?„JþÚ™©ª656ØŸâþ}SAR¼°|'|CñixÄ@y£êÕä>2_}ýÈç"N®µš'"¬Z51^?ZþGOv‰75ŠÄ|°¶Î3SÏÂÙØóÐÙ,RP½î™¬þॠBß•–ÆMGGfõè¨y4ñ²-4ŽuÔNbc@0!ÿç¡+ z Žµˆ¬3"Üg +VÄêêÆzü£GÓkÉg}·ñ"ü˜ #qØk€ÍÛ±c삪䶶ú¶ÄbXàãÅ ô€ÿ¬tá€G„Ç âÑ¢¢XFggÚÊóç½µÀÈbÈ{!çk¨µðf@õ[}Ø[Vïîöz£QùÙZ6ŠP Îʲñ'RÚcOÕÍO€û¡ïu8À샢°+ Ú0嚯:Žv£m Y“kÅ"6r{§å+SNù¶À°ÂþðY \k†·¦9òŸºééš+BÆñãiEÆpÉué\ŒÀlã¹^Dc~<\Y1ƺ  ÷ò’Xð?]cŒ~é8öOcôPg±ÿ©ïn’A´G—IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/right-zmp.png000066400000000000000000000013751207742442300232010ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<zIDATH‰¥”ÏKaÇ?ï̬»¦âjéHA‰!â-déÒ$E ­å‡è?ˆN^»xêÐ-‚³ZØ¿Py­/²ý@$ÓÜÄÄ]wpgžînZ³»¯øŸ™ïû™çÃûŒtK©ä4`Š8Ú]€RÉ&`0€6gC'gè~ 0XÅLB7tÀ0ðø$uCZŠŠz~OAå@îC¡Cäå¯ZYÝ à€8€¦–&]@IÏ;gø JKSM@QÏU`JÄñ÷º’.)uçÄ‘À{zR[fQ“WS“ ¤çm©!òâ=𤦦ª¥’À5è)—–¦Z”nOêÿG¾˜P: `XeŸžR‰L~>Scé*jè)½•âJ%Û àÀrU*)j’Ššª*ê)/ÎG ]mézzʘpQ©¡vm†T¹=ÿ–YÔ ÔTþ›Ú »ÎkõƬˆ¯7ZºÌ|8¬B¹ùüöÎãåçËãÕJ%ï"Îå@@û­öNï¸÷:;˜íìëé ÅŽÅè ÷Òd411=±1ûfv&·žZ{µ¶ ¸ýd¬“"ãkdØ Ûô[ü©Í{›Ýá³áPÔŒb[6Ýánâ qFnŒ´Æ’±~«ÕzTy/AKgxÍÞƒíþí.¿ÁÇ—‡-o‹•Â î/C´'Zgž2oÚ ûLÐñ"“Ÿ€E–Î’:¹îžs#®¸,í.u£ä%O½ª'ãeH»i²ç³FÚ¸< žBP£J݈<Ë—FÖ8Ýì4/à+k5û»c«qf}Κs eP9?‡ïú–a*Ü1Ïî?àpüg¹¤8IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/rotation.png000066400000000000000000000032641207742442300231160ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<1IDATH‰u•kŒUWÇÿkï}sÏ}Ï̽3-Ïaš(AP,(X­øŪiJ%&#4ûÁH±‰-Ú¤‰ÄDÛ¤Ä >Ò´TkÅÔ–Eå1<†Ú™;÷qî=çÜóØË27C3®okïÿ/{­ÿ^›˜³ÅY"S(µÊuhÙÏ6ˆ­-SΓ@Ð’ffRùÅþ@Vï/®¹Ñ#{O^ŸU͸dÛ›Ûþ¸P*ú⧼åõlO³Ñ%O&‚Æêv}`ݼµüwÃœ{âzºÿüÛ™îFûoBìßÖnÿñZâvÑÕtú±Ri‹"úS_óÈsÓQÕQ‰ Í`rÙ}3Ÿê>4¹~áäñëN½¾mõ¿¢´9ï#¶ýð³ŽóƒDòÿÆç'súú†šžGVëßúŒ±*¤;û5U››PrŒCÌLPYsçÄïÙôÒù(š¼¯XÜ$²Ù}³F»º¾2P*½7Uó¼_¶Ûç,mÝ=óp ‚J²;©<õù§.¸0½•ÒÎ+ëçÿü¼ï»÷”Ëk÷‹;n‰®Tj³aºêºW—xÞ+r\`²Ù7³€ %•ùyÑt2²}MxÎ÷ä”âÁ\nËãDN0â8óù¾ïË ŠžS›÷XÚê¨ùÂÏÇïÐÝÖ?FéšçýòL½¯èéÉ”»»¿Ü¤-ë~CJ]sÝ ‹=ï50ØXB ŽÅ >×Dt¦o¡I÷~›ùíQß?e¡Ò逺hš+îèï_®™«N½úÓï P©\q‘€a]‚­L[SÛ¦ª wr;¥çÉ0Œ)ðI=÷8 7øþ Ï[½¼¯oÞ¯òùª½ýßÛ³líúbÙW˜rîØ­#|SJ´z’• ˜ßrL.8zm-kDø€V’ ¥š‹c£þ®nPlZ,b«pmã{F¾î\:¸8\ÓS2ëÎ]ªcɺõ}µßÊ:®V] Rê”ÕWúÛÒ*ˆ0V~B©£ç^ûÀm渭ÑNœ;ã2Ç¢pb ™èêé^Ãr¥½`ÉÙc™=œUŠHK„L‚¥¥”a@ƒ3&¡M‚ò–!ËJˆ‚Õ×·v‰i"Q Ú²™UªÞmšH ‰iPœ’ÙÄ0(¶^˜+R)قه‘ˆoÏYÌzl¶ [ã‡@P=Ž9Že@D„DS[1´d$1skæ IP×Ì ÍÌ` „D" @`3 ‚«”wqLÅHKÙ^Uw]‘–JLÐœdÚdÄ‘F&­Ñ`˜KP¢™t!bH@h3³–aK„¡ºë7Õo¦¾úI·¼ôõ£¨ÝJtX8]|~pòpñs…±¢mTƒråìãĄ̈”J/vçóý7&&^šS­î€ì÷³?î‰z6¸àú”šòkÖ¾6]ÒíÏn¿™ÒÐ Õ™ùøÓ_xú·³Ùn]´èÃÇ\×{þòåM\Ïû›fŽe½{˜hbŠoÌlœÁ†¤=TšÎ5ëÁ™}…Àø"Õ«Ôr0ã¦ëïfTZ­}•Zm4ŸË‰®Lf×-À±ˆ¢Ž ,my’岎©À‹Üv”ÐøBÓ|xM>_z£V‹Æ<ïIàÖ,ZŹÍæþ d!“1ÍÕE§B¶¦L6›b1lüûFÆ‚™7|âHj…eÝ+™ùDµúâ7\÷l }ÿ™›‡¤”:“Ï︸·kYHáðLÅjÌ¿9©[^§ôÅIzßn|wÈ4íç&'/_n4ëئÀ|Ï{âf¥ògK)ÒéÖ\ Æ4éN™rq®é<êlÖ¤wM—§÷èhyë¾—ß¿4ð‡f󟧚Íw3w^鬟þˆmÂ¶í ‘%¬-ŸM6Dva¸e‰ $è†,ÊO/-/-e}ÙS7´¤¸–`+Öj¸¯D,}=â‰VcDbä/Æ› ,£«—¿“ïqú¨žM§¬ú'3«} þ¹.o§œ”˃œù“Ìy®‹ÔùŒþ=”k”{2Ó†ûL2ö o•YYÂYÆG¹\#ÕD‰èfÑtMáS~þ‡Ý,:Ïij<û‚ò ckfõC8qÿ oã¶7øâvSW ˆdárÞÝɺa~úÍ»2¿¬_˜òbÖåïð/ÖeVg~o¯EÜ·’#ãŒ<•¹±YoFœYÀÒ‹lz>ó«™`ãñ%L\ÀÉjØ={8<—yêWú'8Ýêsözg¯ç:4F¼€[þ1ÆÉƼIEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/startsimulation.png000066400000000000000000000025561207742442300245240ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ëIDATH‰–[lTeÇóÓÒ+±¶…‚ØîžÕj4¢-¡Öª&Š b‚÷KõÁDM·!Q¼dQߌ/š ‰Š0´˜1 Ö@1K…ž=ŤB/ô¶º»t÷Œ´XdµÄyœùæÿ›™‡™OT'ìܬØlq#îS,…}ü\o£÷ŠN&Nc¢ª8ag#°.¯(/m[6ñѸ%ÈiU} x͸)§Åù¥x/¶1öä…@$øb° ˆÍ¾r¶\qí[ÅHRð~ñ8|ø°ŠÈQß÷ow#n—Óâü Ô €²)öFìñé hK´âªŠ³Î¢B×/¦ie“ø¾_ ,žŒå—åS*áÑÀºÀ&Ù f:À5ù¥ùbåZ Ÿ&Ö£+ÚE*•¢¼¬üü–ªêª˜{ù\Däá`2¸ù¿ ¶¹Ä.°E}%úmÔ÷Ó¾ý)ªõuõV¶¤Ù¹³)_\N®ÉÅëò&ƒFšä!ݦ™lTØy6c'ÇðÓ¾®‚étzË÷íßg­jÎŒ9iƒãƒ´ ´ÑPÒÀÊÆ•ÜZ+*zçPéЛƸqU}*Þ·Žì=¢¾fÜuuuÔ\]ƒˆ¼î„˧ƒŒ¦GÙ3¸‡D&qŽßLŒ`'ðH_OŸßÞÖž‰Æ¸¾þz,Ë2ÀÚé)?EÛmìnߨì,,yÁ8açæ Èf”›GGú¾úì«ô©ÁSäæäª cÌØóþÞ÷5ªº£d¨¤©ãÝŽq¼í„û& ߪê‚ñññ£­­­™t&MEY… TO'þ]ûwì=°`{ ?pOÇ»ãFŒä8agŤ_UŸ8}ú´Õ?ÐOaA!¾úùNØ™ñoâííítìØZ•Wuß7/}“žŒÎì/ áS'쬞ðŸ#''gòmQ6ñ?àСC(úI¬;¶fª8€ 0³r&šT{ìäØöPKh?wÉ<ñŽypæÊ ÿS<úc¯ËøÈëö².;@,á²%—™ToŠã‡Ž_c›ÊK+¥ ¿€øŸqŒ1#Ç^?–qZœ³‰¿îû•Þ£½Äòbë6=oتªÉSIM§ÒRzi)@À*²‹(¶Š81pBØd‚¢ôìïa {UÝìå{éKÙÅ Ês©‘ÔŸ_vfFûGÏ  ãwÅ÷ý÷'}‰Ý lòÞðý/qøûèW‹ÈçªZ}QÙE™ÊªJ+ÏÊ#¦‰DRUç¹wäý)ß–ànyX¨ª–³ÏWq»B-¡ç-Ebë/ôÛò<¤I†“fÕ˜IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/stdpose.png000066400000000000000000000025241207742442300227360ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ÑIDATH‰•–[LTW†ÿu8s¿p`å"¬Nª2ÚB4éE¬Œ15éKÕ´QÛÔzi 1v:MZ/mZcúPûÐØ&†`$ŠPÛšb-I#¥¶V¤0s8Ì•†¹ì>p Žb`=íœó¯ÿ[gŸµv61Æ°ÈÍ=sV&K«‹—¤C甀[ˆˆÈ™ŸŸýêþýÕ¥Z­òàBÍ  Ñj•2Ëeüb³b"çZ²yt¹3‹D"É97Ì£ó1æøwî>R˜LÂu«µ¨L¡àÓ櫤°Ð¤5› ÜÝ»ýÑ@ }¢»ot¢§ÇÓ64ô_ÏÎNÏÛ¸qUÆ|æs£¬,_@1Ïký;}Õ&Óé:¯÷½À3æH˜L§ÿ ffêpíÚc÷ïº ÙøLV"‘L[³¦x¹ÍfÑÖ×ßðƒc©ÎñxBUSc[aµ©<: ÑœhŽDêºxÅàëõõ7:öî}ÙRUµR}ïÞ@¸·×ócŽ9 ‹‹snÐŽŽNˆ==ûÖ¥ˆœ²K—Ú;öìyÉj·¯ÏÅà"çZs%)ôÖ•+·¼J¥›7¯}N4ï,dËf‚1GÌãñ¿ÖÐp³ŸçÓ°}ûóÏé§fÛtl,zÓåÉ$CF†V.—óK˜Ú¦c<ÿP4ƒÁ #"*˜ º·lYWÀq„ÖÖîÿFFÂç ÐéNn³ÙJJ Z[ÿôE"g8 ræ/Y’¹Ëb1+].oÌí/ÄãÇî-Ɯȩ4tŸVT”føý£èîvÿ×þÌ@NNÆ·vûú‚x<¦¦ÛKR¨N¥:nÓëUGðƒNÅq$€X,že2¾ Œ! F>‰FvdeéOÕÔØ,D„ÆÆö~Q ¾ <‘³ ¢¢´D­V ¹ù¶gd$¼G¥’o\µªà»­[mF>eööí{%Ó“‹ÅqîÜOåDÎâ’su^^&ßÝÝ?!Iá¯sˆÀÔ ùz{½áË—÷öôx¾Ÿ˜˜ì0› vûc,ÇðppÖœ1€hjm4 Éxh4Ê(€´H$íêr%®_ïêòùÂ'grxÆa"gùà /‹1GŸ |¶»²r¥…ãín—K< `z(™L29ÏsÞf”¤¶1æˆ9_lhøu],–ø1Gr0U™# Lääóó³k­Ö"­(“#?úýGŽÏÝ¢ÌÌ/>°Û× D„¦¦Ž^I šöp9õç?r\§§kvVU=[L\½ÚÑ'I¡÷SôzÕn‹Å,w»Å˜(/2æðÍÓX8ŽŒZ­RƉD#B© ii|r2~ãiæüþÑoÚÚþr••eËôzõ© áðx»ß?Šòò½ h>^€1Ç°×hñzlõê"­ hŽ9éSQ liéts¡¢¢Ô"ŸïZ0$)T×ÒÒÙGlÚ´z¹ hÞN)Bò_2«u™&=]]›ZÄSÓ'ëŇ¥ÄŠy*¥R¾#U#I¡ÚææÎ^"`éÒ,€¼ùô¤k ‘Sm4¦ÿ¢Ó©ÍÀèII:ôUª&;ûËZ­òØøødרžÛûsãªÚƒ?;¨IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/stopsimulation.png000066400000000000000000000005361207742442300243500ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ÛIDATH‰í–1jQEÏ}?l¦²³„,À¤\k°O!¸Œ”!¸ìA,‰ ‚ŽâK%:fÀNø¯¹ÿÝâžâ÷ËÝ©s¬Ötàézé ;/&ûÄiþ5@èzùvó×ÙÛl^  wn4 HÂ0$!.ï’"LFq(Xo×-ÕF•€ó´»mò<'†Hf1D¢E²]ôƛΧŒ?Æ¿²j¿A$@$@5 ]âwÊÅÿŠàŸ–nÓÔoÝnJ++koÜ`}}îAÜÉ,¬A9À,àBv6râDÎsç²&…ÃRòT0húõ«Êp ײ8åóiqUÕý"š³gÏ#ëDôˆãȽ¡ªÜíƒæ‹8× C[@ƒ"šÕLfY7.þgmmçÝ»»÷ææÚ·LÓ9êIÆTÕû5yyÚ»oß_ï,_>¼6¡xéÒPs²my?]Ÿ,|¦ImYYðü”)¶gõê!•"\-I=ëhÎnÝõÖ Û4[ÆE¦tæ'Þãvbâzà¬*2 Á2 6Ž›eË‚Ýuu¹³o߶òm›C¦É‚–åË'ôtä;ÎŒ"¹) žF—æsåH?K–Û­S‹‹ã®¦&×ãÇŽyŸ°m^JAhmµ<¶-Þø­Æ‘‰;]#8ÎÝži";D‚ï¸ZÞ“·ˆÌݬڙCK,K{@ËÒê~E­IUL.íº…,µÓÈé8Ä>€£©ÊIµ¤DMЂ$hú¹Çãüæó9§Aà›Þ¥è|îÄu ß(èNVÃÌŒÔ'â„V¬è«<|¸kÖСΠÕS¨1Ÿuñmža{ 3L!iÝéŒ45{#?_ÛÊÊBñ@@Æûýòpê"K7 ù°ùÉUþ¢I~ÄuÏa‡â‘ìÁÁ p¹´rÎœðï":æèѬy"tªr6¿R¸)rÏ€òA8PQꇥn7c. ]ÝÐUšHðñsòÅ‚—Í_'Loít·ÔhaߥQ…a‚ÒäõûéõÕ"M€Óo?¾MÇx½jÛ6£¯]3íè0 :/]kž±?³Fž³Lð¤8Îß(x Ðè/ n¨Ü`oV–ó“Ë¥Qlj¸\Z“ħ¹¾¹¼¯þñ°°_aµÜG™^»è4ÐY ’Š/bÉš­˜á›‚ÇA¿„à¨?¦ªf~23­-"/ºáøÈŸ‚Ç:u±¼¾Yõ®2Màì flƒÀÕ”aÛíð•ö¿óIûϤdò¼ N¿¹ 솃àMüßý$ÏZpÆ„¯7¨®Nwæoµâ7é¯@IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/icons/zmp-to-cm.png000066400000000000000000000021231207742442300230730ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ÐIDATH‰”[L\U†¿5g˜ážRèŒx£D[¡Ø”DëíA¤1ð°'LŒ¾˜Ô4Ѧ`Ñ*ÅT£ }hMŠ!Õ¤V0N›”˜6ZËõI°µÕ: tZ M¹Í0ˇÎè”ÂLì9ÙÉ9{¯ýÿZûß[T•dO«HƧ³uUnn‰¤§Û‚33}W¯îlVJ†•dŸŠäÛlÝ5UUÎÊJ(.&¤Êíí>ŸÏWýîÌL ÞžLÁCðÅK.W…=?  ¢‚”ìl¶fd”ÎìÞÝÔ$ÂÛ’¸`]ä¿ UˆDÜyy%-" E&\l±m4&'áÚ5„”p8`hˆÌP(Í YÀä=4«F¾en®¿,ë6‘eÁŒ£»TWLž” €uè<òdùð°`òòÀ²¸<>©W‚ÓG“áQÕ„̶gÙ¨]؇a. '±oe½‚iL†OhSOÐüµ‡c/¤A…€Ó=;0Ç* HÕ;¾RŽd.jÖ Íª ³ÐÓ úó;ª³ÑµL ùžZ&Ì-0±¹o¡ë8Ó&¦x¥<‰*ø°€÷ŽŠ|V 5ÏÀ_‹ìˆÆ4³À¾ÿU˜ÇÀ„ÁìWU¾‚W`Bo3ýÆCm4¶ Œ‚Ù²l®N‚™“sJ…þXòØè…þ6Ø&Ì0¿‘¤`žŽ*Ú¹2OAïÒä±Ñ >7˜ú(æÕ„6ñÐuNÉ!Ž=Ÿõv…¡¤*~‚s pQAFàÌ›˜/£87P¢ê]vÀ¼UR¿TIˆ)?ËTþTûÞ².ñ8€O€s@ç]fˆû»ÖÕ{èv‰x\±ùx›¾ªÞ;Ž·»Î-—ÜŽÜÃëÒi_ŸÉ%—#Ç]ç^Îâ@*Ðò¯UEij èzT½ÕK’„ïwf•:Ÿ(-/w¥1í»ê=ûÇ…‰‘‰×G¾¹/â9û¸ª×ëß>0‹`6Ä÷ÏUëº/çíœ9+º¹³66é雧Õ?ï׎ñ-û°ì²«ÖU¸d/Vƒ™sJU±‰x¶GT½Æ« »ÂG‚ž`¡:•Ø!BHCXv‹M¯mZk=`Ýqe«z'€½À‹"žçìÑ>XÒš”pYø‘Hv€áÐ0þ?qà³|Œ†G Xæœ_ë®s¯;16ÿØì·{U½ÃK6lƒ5iegþ90šNû}ñgŸu±RB7¡[S‹SD®GÒ€  ;®ŠÏv õ@Â}¢â„IEND®B`‚choreonoid-1.1.0+dfsg/src/BodyPlugin/po/000077500000000000000000000000001207742442300200475ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/BodyPlugin/po/ja.po000066400000000000000000000373071207742442300210130ustar00rootroot00000000000000# Language = translations for PACKAGE package. # Copyright (C) 2011 THE PACKAGE'S COPYRIGHT HOLDER # This file is distributed under the same license as the PACKAGE package. # nakaoka , 2011. # msgid "" msgstr "" "Project-Id-Version: PACKAGE VERSION\n" "Report-Msgid-Bugs-To: \n" "POT-Creation-Date: 2011-12-21 16:15+0000\n" "PO-Revision-Date: 2011-11-12 14:46+0000\n" "Last-Translator: nakaoka \n" "Language-Team: Language =\n" "Language: =\n" "MIME-Version: 1.0\n" "Content-Type: text/plain; charset=UTF-8\n" "Content-Transfer-Encoding: 8bit\n" #: BodyBar.cpp:27 msgid "Memory the current pose" msgstr "ç¾åœ¨å§¿å‹¢ã‚’記憶" #: BodyBar.cpp:30 msgid "Recall the memorized pose" msgstr "記憶ã—ãŸå§¿å‹¢ã®å‘¼ã³å‡ºã—" #: BodyBar.cpp:33 msgid "Move the selected bodies to the origin" msgstr "é¸æŠžãƒœãƒ‡ã‚£ã‚’原点ã¸ç§»å‹•" #: BodyBar.cpp:36 msgid "Set the preset initial pose to the selected bodies" msgstr "é¸æŠžãƒœãƒ‡ã‚£ã‚’åˆæœŸå§¿å‹¢ã«" #: BodyBar.cpp:39 msgid "Set the preset standard pose to the selected bodies" msgstr "é¸æŠžãƒœãƒ‡ã‚£ã‚’標準姿勢ã«" #: BodyBar.cpp:44 msgid "Copy the right side pose to the left side" msgstr "å³å´ã®å§¿å‹¢ã‚’å·¦å´ã«ã‚³ãƒ”ー" #: BodyBar.cpp:47 msgid "Mirror copy" msgstr "å転コピー" #: BodyBar.cpp:50 msgid "Copy the left side pose to the right side" msgstr "å·¦å´ã®å§¿å‹¢ã‚’å³å´ã«ã‚³ãƒ”ー" #: BodyBar.cpp:56 msgid "" "Move the center of mass to the position where its projection corresponds to " "the support feet cener" msgstr "é‡å¿ƒæŠ•å½±ç‚¹ãŒæ”¯æŒè„šä¸­å¿ƒã«ä¸€è‡´ã™ã‚‹ã‚ˆã†ç§»å‹•" #: BodyBar.cpp:59 msgid "Move the center of mass to fit its projection to ZMP" msgstr "é‡å¿ƒæŠ•å½±ç‚¹ãŒZMPã«ä¸€è‡´ã™ã‚‹ã‚ˆã†ç§»å‹•" #: BodyBar.cpp:64 msgid "Set ZMP to the projection of the center of mass" msgstr "ZMPã‚’é‡å¿ƒæŠ•å½±ç‚¹ã«ç§»å‹•" #: BodyBar.cpp:67 msgid "Set ZMP under the right foot" msgstr "ZMPã‚’å³è¶³ã«ç§»å‹•" #: BodyBar.cpp:70 msgid "Set ZMP at the center of the feet" msgstr "ZMPを両足中心ã«ç§»å‹•" #: BodyBar.cpp:73 msgid "Set ZMP under the left foot" msgstr "ZMPを左足ã«ç§»å‹•" #: BodyBar.cpp:78 msgid "Adjust the width between the feet" msgstr "足幅ã®èª¿æ•´" #: BodyBar.cpp:83 msgid "Width between the feet [m]" msgstr "両足中心間è·é›¢" #: BodyBar.cpp:253 msgid "The center of mass of %1% cannt be moved to the target position\n" msgstr "%1% ã®é‡å¿ƒã‚’指定ã®ä½ç½®ã¾ã§ç§»å‹•ã§ãã¾ã›ã‚“。\n" #: BodyItem.cpp:76 msgid "BodyItem" msgstr "" #: BodyItem.cpp:78 msgid "OpenHRP model file" msgstr "OpenHRP モデルファイル" #: BodyItem.cpp:818 msgid "Model name" msgstr "モデルå" #: BodyItem.cpp:819 msgid "Num links" msgstr "リンク数" #: BodyItem.cpp:820 msgid "Num joints" msgstr "関節数" #: BodyItem.cpp:821 msgid "Root link" msgstr "ルートリンク" #: BodyItem.cpp:822 msgid "Base link" msgstr "ベースリンク" #: BodyItem.cpp:823 msgid "Mass" msgstr "質é‡" #: BodyItem.cpp:824 msgid "Static model ?" msgstr "é™æ­¢ãƒ¢ãƒ‡ãƒ«ï¼Ÿ" #: BodyItem.cpp:825 msgid "Model file" msgstr "モデルファイル" #: BodyItem.cpp:827 msgid "Self-collision" msgstr "自己干渉" #: BodyLinkView.cpp:151 msgid "Body / Link" msgstr "ボディï¼ãƒªãƒ³ã‚¯" #: BodyLinkView.cpp:214 msgid "Index:" msgstr "インデックス" #: BodyLinkView.cpp:216 msgid "Joint ID:" msgstr "関節ID" #: BodyLinkView.cpp:218 msgid "Joint Type:" msgstr "関節タイプ" #: BodyLinkView.cpp:220 msgid "Joint Axis:" msgstr "関節軸" #: BodyLinkView.cpp:261 msgid "min" msgstr "最å°" #: BodyLinkView.cpp:271 msgid "max" msgstr "最大" #: BodyLinkView.cpp:291 msgid "Link Position [m],[deg]" msgstr "リンクä½ç½® [m],[deg]" #: BodyLinkView.cpp:331 msgid "Matrix" msgstr "行列" #: BodyLinkView.cpp:362 msgid "ZMP [m]" msgstr "ZMP [m]" #: BodyLinkView.cpp:389 msgid "Collisions" msgstr "干渉" #: BodyLinkView.cpp:403 msgid "Self-Collisions" msgstr "自己干渉" #: BodyLinkView.cpp:547 msgid "Free" msgstr "éžæ‹˜æŸ" #: BodyLinkView.cpp:550 msgid "Fixed" msgstr "固定" #: BodyLinkView.cpp:554 msgid "Rotation" msgstr "回転" #: BodyLinkView.cpp:556 msgid "Joint Angle [deg]" msgstr "関節角 [deg]" #: BodyLinkView.cpp:568 msgid "Joint Velocity [deg/s]" msgstr "関節角速度 [deg/s]" #: BodyLinkView.cpp:582 msgid "Slide" msgstr "ç›´å‹•" #: BodyLinkView.cpp:584 msgid "Joint Translation [m]:" msgstr "関節並進ä½ç½® [m]" #: BodyLinkView.cpp:593 msgid "Joint Velocity [m/s]" msgstr "関節速度 [m/s]" #: BodyMotionItem.cpp:27 msgid "Warning" msgstr "警告" #: BodyMotionItem.cpp:67 msgid "" "The frame rate of a body motion exported as Hrpsys files should be standard " "value 200, but the frame rate of \"%1%\" is %2%. The exported data may cause " "a problem.\n" "\n" "Do you continue to export ?" msgstr "" "Hrpsys用ファイルã®ãƒ•ãƒ¬ãƒ¼ãƒ ãƒ¬ãƒ¼ãƒˆã¯æ¨™æº–ã§200ã¨ãªã£ã¦ã„ã¾ã™ãŒã€ã‚¨ã‚¯ã‚¹ãƒãƒ¼ãƒˆã—よ" "ã†ã¨ã—ã¦ã„ã‚‹ \"%1%\" ã®ãƒ•ãƒ¬ãƒ¼ãƒ ãƒ¬ãƒ¼ãƒˆã¯%2%ã¨ãªã£ã¦ãŠã‚Šã€ã“ã‚Œã«ã‚ˆã£ã¦å•é¡ŒãŒèµ·" "ã“ã‚‹å¯èƒ½æ€§ãŒã‚ã‚Šã¾ã™ã€‚" #: BodyMotionItem.cpp:83 msgid "" "A fault has been detected. Please check the report in the MessageView.\n" "\n" "Do you continue to export ?" msgstr "" "障害を検出ã—ã¾ã—ãŸã€‚メッセージビュー内ã«å‡ºåŠ›ã•ã‚Œã‚‹å†…容を確èªã—ã¦ãã ã•ã„。\n" "\n" "エクスãƒãƒ¼ãƒˆã‚’続ã‘ã¾ã™ã‹ ?" #: BodyMotionItem.cpp:85 msgid "" "%1% faults have been detected. Please check the report in the MessageView.\n" "\n" "Do you continue to export ?" msgstr "" "%1% 箇所ã®éšœå®³ã‚’検出ã—ã¾ã—ãŸã€‚メッセージビュー内ã«å‡ºåŠ›ã•ã‚Œã‚‹å†…容を確èªã—ã¦ã" "ã ã•ã„。\n" "\n" "エクスãƒãƒ¼ãƒˆã‚’続ã‘ã¾ã™ã‹ ?" #: BodyMotionItem.cpp:103 msgid "There is no ZMP data. Do you continue to export ?" msgstr "ZMPã®ãƒ‡ãƒ¼ã‚¿ãŒã‚ã‚Šã¾ã›ã‚“ãŒã€ã‚¨ã‚¯ã‚¹ãƒãƒ¼ãƒˆã‚’続ã‘ã¾ã™ã‹ï¼Ÿ" #: BodyMotionItem.cpp:156 msgid "BodyMotionItem" msgstr "ボディモーションアイテム" #: BodyMotionItem.cpp:158 msgid "Number of joints" msgstr "関節数" #: BodyMotionItem.cpp:163 msgid "Body Motion" msgstr "ボディモーション" #: BodyMotionItem.cpp:167 msgid "Hrpsys sequence file set" msgstr "Hrpsys シーケンスファイル一å¼" #: BodyPlugin.cpp:76 msgid "Body Plugin Version %1%\n" msgstr "Body Plugin ãƒãƒ¼ã‚¸ãƒ§ãƒ³ %1%\n" #: BodyPlugin.cpp:78 msgid "" "This plugin has been developed by Shin'ichiro Nakaoka and Choreonoid " "Development Team, AIST, and is distributed as a part of the Choreonoid " "package.\n" "\n" msgstr "" #: BodyPlugin.cpp:83 msgid "" "The Collision deteciton module used in this plugin is implemented using the " "OPCODE library (http://www.codercorner.com/Opcode.htm).\n" msgstr "" #: DynamicsSimulatorItem.cpp:93 msgid "DynamicsSimulatorItem" msgstr "動力学シミュレータアイテム" #: DynamicsSimulatorItem.cpp:524 msgid "Static friction" msgstr "é™æ­¢æ‘©æ“¦ä¿‚æ•°" #: DynamicsSimulatorItem.cpp:526 msgid "Slip friction" msgstr "動摩擦係数" #: FilterDialogs.cpp:47 FilterDialogs.cpp:304 msgid "Velocity Limiting Filter" msgstr "速度制é™ãƒ•ã‚£ãƒ«ã‚¿ãƒ¼" #: FilterDialogs.cpp:54 msgid "Selected joints only" msgstr "é¸æŠžé–¢ç¯€ã®ã¿" #: FilterDialogs.cpp:70 msgid "Pollard method" msgstr "Pollard手法" #: FilterDialogs.cpp:87 msgid "Gaussian Filter" msgstr "ガウシアンフィルタ" #: FilterDialogs.cpp:109 FilterDialogs.cpp:213 msgid "Apply" msgstr "é©ç”¨" #: FilterDialogs.cpp:110 FilterDialogs.cpp:214 msgid "Cancel" msgstr "キャンセル" #: FilterDialogs.cpp:180 FilterDialogs.cpp:308 msgid "Range Limiting Filter" msgstr "範囲制é™ãƒ•ã‚£ãƒ«ã‚¿" #: FilterDialogs.cpp:188 msgid "limit grad." msgstr "制約勾é…" #: FilterDialogs.cpp:195 msgid "edge grad. ratio" msgstr "辺勾é…比率" #: FilterDialogs.cpp:202 msgid "margin" msgstr "マージン" #: FilterDialogs.cpp:311 msgid "Merge selected MultiValueSeqItems (Temporary version)" msgstr "é¸æŠžã—ãŸMultiValueSeqアイテムをマージ（試験的ãƒãƒ¼ã‚¸ãƒ§ãƒ³ï¼‰" #: JointSliderView.cpp:231 msgid "Joint Sliders" msgstr "関節スライダ" #: JointSliderView.cpp:241 msgid "All" msgstr "全関節" #: JointSliderView.cpp:242 msgid "Show all the joints including unselected ones" msgstr "全関節を表示" #: JointSliderView.cpp:248 msgid "ID" msgstr "" #: JointSliderView.cpp:249 msgid "Show joint IDs" msgstr "関節IDを表示" #: JointSliderView.cpp:255 msgid "Name" msgstr "åå‰" #: JointSliderView.cpp:256 msgid "Show joint names" msgstr "関節åã®è¡¨ç¤º" #: JointSliderView.cpp:262 msgid "Entry" msgstr "数値入力" #: JointSliderView.cpp:263 msgid "Show spin entries for numerical input" msgstr "数値入力ボックスã®è¡¨ç¤º" #: JointSliderView.cpp:269 msgid "Slider" msgstr "スライダ" #: JointSliderView.cpp:270 msgid "Show sliders for chaning joint positions" msgstr "スライダを表示" #: JointSliderView.cpp:276 msgid "IL" msgstr "一列" #: JointSliderView.cpp:277 msgid "Put all the components for each joint in-line" msgstr "å„関節を一列ã«è¡¨ç¤º" #: JointSliderView.cpp:287 msgid "The number of columns" msgstr "カラム数" #: KinematicFaultChecker.cpp:102 KinematicFaultChecker.cpp:136 msgid "Kinematic Fault Checker" msgstr "é‹å‹•å­¦éšœå®³ãƒã‚§ãƒƒã‚«" #: KinematicFaultChecker.cpp:143 msgid "Joint position check" msgstr "関節ä½ç½®ãƒã‚§ãƒƒã‚¯" #: KinematicFaultChecker.cpp:148 msgid "Angle margin" msgstr "角度マージン" #: KinematicFaultChecker.cpp:156 msgid "Translation margin" msgstr "並進é‡ãƒžãƒ¼ã‚¸ãƒ³" #: KinematicFaultChecker.cpp:167 msgid "Joint velocity check" msgstr "関節速度ãƒã‚§ãƒƒã‚¯" #: KinematicFaultChecker.cpp:172 msgid "Limit ratio" msgstr "仕様制約値ã«å¯¾ã™ã‚‹å‰²åˆ" #: KinematicFaultChecker.cpp:187 msgid "All joints" msgstr "全関節" #: KinematicFaultChecker.cpp:191 msgid "Selected joints" msgstr "é¸æŠžé–¢ç¯€" #: KinematicFaultChecker.cpp:194 msgid "Non-selected joints" msgstr "éžé¸æŠžé–¢ç¯€" #: KinematicFaultChecker.cpp:202 msgid "Self-collision check" msgstr "自己干渉ãƒã‚§ãƒƒã‚¯" #: KinematicFaultChecker.cpp:212 msgid "Time bar's range only" msgstr "タイムãƒãƒ¼ã®ç¯„囲内ã«é©ç”¨" #: KinematicFaultChecker.cpp:220 msgid "&Apply" msgstr "é©ç”¨(&A)" #: KinematicFaultChecker.cpp:274 msgid "No BodyMotionItems are selected." msgstr "BodyMotionアイテムãŒé¸æŠžã•ã‚Œã¦ã„ã¾ã›ã‚“。" #: KinematicFaultChecker.cpp:280 msgid "%1% is not owned by any BodyItem. Check skiped." msgstr "%1% ã¯ãƒœãƒ‡ã‚£ã‚¢ã‚¤ãƒ†ãƒ ã®æ‰€æœ‰ã§ã¯ã‚ã‚Šã¾ã›ã‚“ã®ã§ã€ãƒã‚§ãƒƒã‚¯ã‚’ã—ã¾ã›ã‚“。" #: KinematicFaultChecker.cpp:283 msgid "Applying the Kinematic Fault Checker to %1% ..." msgstr "%1%ã«å¯¾ã—ã¦é‹å‹•å­¦çš„障害ãƒã‚§ãƒƒã‚«ã‚’é©ç”¨ä¸­â€¦" #: KinematicFaultChecker.cpp:314 msgid "A fault has been detected." msgstr "障害ãŒæ¤œå‡ºã•ã‚Œã¾ã—ãŸã€‚" #: KinematicFaultChecker.cpp:316 msgid "%1% faults have been detected." msgstr "%1%ã®éšœå®³ãŒæ¤œå‡ºã•ã‚Œã¾ã—ãŸã€‚" #: KinematicFaultChecker.cpp:319 msgid "No faults have been detected." msgstr "障害ã¯æ¤œå‡ºã•ã‚Œã¾ã›ã‚“ã§ã—ãŸã€‚" #: KinematicFaultChecker.cpp:459 msgid "" "%1$7.3f [s]: Position limit over of %2% (%3% is beyond the range (%4% , %5%) " "with margin %6%.)" msgstr "" "%1$7.3f [秒]: %2%ã®é–¢ç¯€ä½ç½®åˆ¶ç´„オーãƒãƒ¼ (%3% ã¯ç¯„囲(%4% , %5%)ã®ãƒžãƒ¼ã‚¸ãƒ³%6%ã‚’" "超ãˆã¦ã„ã¾ã™ã€‚" #: KinematicFaultChecker.cpp:460 msgid "" "%1$7.3f [s]: Position limit over of %2% (%3% is beyond the range (%4% , " "%5%).)" msgstr "" "%1$7.3f [秒]: %2%ã®é–¢ç¯€ä½ç½®åˆ¶ç´„オーãƒãƒ¼ (%3% ã¯ç¯„囲(%4% , %5%)を超ãˆã¦ã„ã¾" "ã™ã€‚" #: KinematicFaultChecker.cpp:490 msgid "" "%1$7.3f [s]: Velocity limit over of %2% (%3% is %4$.0f %% of the range " "(%5% , %6%).)" msgstr "" "%1$7.3f [秒]: %2%ã®é–¢ç¯€é€Ÿåº¦ã‚ªãƒ¼ãƒãƒ¼ (%3% ã¯ç¯„囲(%5% , %6%)ã®%4$.0f %%を超ãˆã¦" "ã„ã¾ã™ã€‚" #: KinematicFaultChecker.cpp:517 msgid "%1$7.3f [s]: Collision between %2% and %3%" msgstr "%1$7.3f [秒]: %2% 㨠%3% ã®å¹²æ¸‰ã‚’検出" #: KinematicsBar.cpp:39 msgid "Kinematics Operation Setup" msgstr "é‹å‹•å­¦æ“作ã®è¨­å®š" #: KinematicsBar.cpp:45 msgid "Snap thresholds:" msgstr "スナップ閾値" #: KinematicsBar.cpp:48 msgid "distance" msgstr "足幅" #: KinematicsBar.cpp:55 KinematicsBar.cpp:74 msgid "[m]" msgstr "" #: KinematicsBar.cpp:58 msgid "angle" msgstr "角度" #: KinematicsBar.cpp:63 msgid "[deg]" msgstr "" #: KinematicsBar.cpp:67 msgid "Penetration block depth" msgstr "貫通防止ã«ãŠã‘る許容深度" #: KinematicsBar.cpp:78 msgid "Lazy collision detection mode" msgstr "é…延干渉検出モード" #: KinematicsBar.cpp:84 msgid "OK" msgstr "了解" #: KinematicsBar.cpp:119 msgid "KinematicsBar" msgstr "é‹å‹•å­¦ãƒãƒ¼" #: KinematicsBar.cpp:125 msgid "Forward kinematics mode" msgstr "é †é‹å‹•å­¦ãƒ¢ãƒ¼ãƒ‰" #: KinematicsBar.cpp:126 msgid "Preset kinematics mode" msgstr "プリセットé‹å‹•å­¦ãƒ¢ãƒ¼ãƒ‰" #: KinematicsBar.cpp:127 msgid "Inverse kinematics mode" msgstr "逆é‹å‹•å­¦ãƒ¢ãƒ¼ãƒ‰" #: KinematicsBar.cpp:132 msgid "Enable link orientation editing" msgstr "リンク姿勢編集ã®è¨±å¯" #: KinematicsBar.cpp:146 msgid "Penetration block mode" msgstr "貫通ブロックモード" #: KinematicsBar.cpp:149 msgid "Highlight colliding links" msgstr "干渉リンクã®ãƒã‚¤ãƒ©ã‚¤ãƒˆè¡¨ç¤º" #: KinematicsSimulatorItem.cpp:80 msgid "KinematicsSimulatorItem" msgstr "é‹å‹•å­¦ã‚·ãƒŸãƒ¥ãƒ¬ãƒ¼ã‚¿ã‚¢ã‚¤ãƒ†ãƒ " #: LinkSelectionView.cpp:57 msgid "Links" msgstr "リンク" #: LinkTreeWidget.cpp:259 msgid "link" msgstr "リンク" #: LinkTreeWidget.cpp:263 msgid "id" msgstr "" #: LinkTreeWidget.cpp:281 msgid "link list" msgstr "リンク一覧" #: LinkTreeWidget.cpp:282 msgid "link tree" msgstr "リンクツリー" #: LinkTreeWidget.cpp:283 msgid "joint list" msgstr "関節一覧" #: LinkTreeWidget.cpp:284 msgid "joint tree" msgstr "関節ツリー" #: LinkTreeWidget.cpp:285 msgid "part tree" msgstr "身体部ä½ãƒ„リー" #: MultiAffine3SeqGraphView.cpp:27 msgid "Multi Affine3 Seq" msgstr "ä½ç½®å§¿å‹¢è»Œé“グラフ" #: MultiValueSeqGraphView.cpp:29 msgid "Multi Value Seq" msgstr "軌é“グラフ" #: SceneBody.cpp:1049 msgid "%1% / %2%" msgstr "%1% / %2%" #: SceneBody.cpp:1109 msgid "Set Free" msgstr "フリーã«" #: SceneBody.cpp:1111 msgid "Set Base" msgstr "ベースã«" #: SceneBody.cpp:1113 msgid "Set Translation Pin" msgstr "並進ピンをセット" #: SceneBody.cpp:1115 msgid "Set Rotation Pin" msgstr "回転ピンをセット" #: SceneBody.cpp:1117 msgid "Set Both Pins" msgstr "両方ã®ãƒ”ンをセット" #: SceneBody.cpp:1122 msgid "Level Attitude" msgstr "水平姿勢ã«" #: SceneBody.cpp:1127 msgid "/Markers" msgstr "/マーカー" #: SceneBody.cpp:1129 msgid "Center of Mass" msgstr "é‡å¿ƒ" #: SceneBody.cpp:1133 msgid "ZMP" msgstr "ZMP" #: SceneBodyManager.cpp:112 msgid "Show only selected links" msgstr "é¸æŠžãƒªãƒ³ã‚¯ã®ã¿è¡¨ç¤ºã™ã‚‹" #: SimulationBar.cpp:25 msgid "Simulation" msgstr "シミュレーション" #: SimulationBar.cpp:27 msgid "Output all link positions" msgstr "全関節ä½ç½®å§¿å‹¢ã®SE3軌é“を出力" #: SimulationBar.cpp:47 SimulationBar.cpp:111 msgid "Start simulation" msgstr "シミュレーション開始" #: SimulationBar.cpp:86 msgid "Stop simulation" msgstr "シミュレーションåœæ­¢" #: SimulatorItem.cpp:130 msgid "Simulation by %1% has started." msgstr "%1%ã«ã‚ˆã‚‹ã‚·ãƒŸãƒ¥ãƒ¬ãƒ¼ã‚·ãƒ§ãƒ³ã‚’開始ã—ã¾ã—ãŸã€‚" #: SimulatorItem.cpp:180 msgid "Simulation by %1% has finished at %2% [s].\n" msgstr "%1%ã«ã‚ˆã‚‹ã‚·ãƒŸãƒ¥ãƒ¬ãƒ¼ã‚·ãƒ§ãƒ³ãŒçµ‚了ã—ã¾ã—ãŸã€‚\n" #: SimulatorItem.cpp:181 msgid " Actual elapsed time = %1% [s], actual / virtual = %2%." msgstr "実消費時間 = %1% [s], 実時間 / 仮想時間 = %2%。" #: WorldItem.cpp:102 msgid "WorldItem" msgstr "ワールドアイテム" #: WorldItem.cpp:399 WorldView.cpp:76 msgid "Collision detection" msgstr "干渉検出" #: WorldView.cpp:63 WorldView.cpp:69 msgid "World" msgstr "ワールド" choreonoid-1.1.0+dfsg/src/CMakeLists.txt000066400000000000000000000020061207742442300201130ustar00rootroot00000000000000 # @author Shin'ichiro Nakaoka #set(CMAKE_BUILD_TYPE Debug) if(ENABLE_INSTALL_RPATH) set(CMAKE_INSTALL_RPATH "$ORIGIN") endif() add_subdirectory(Util) add_subdirectory(Base) add_subdirectory(Collision) add_subdirectory(Body) if(ENABLE_INSTALL_RPATH) set(CMAKE_INSTALL_RPATH "$ORIGIN/../lib") else() unset(CMAKE_INSTALL_RPATH) endif() add_subdirectory(Choreonoid) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CNOID_PLUGIN_SUBDIR}) if(MSVC) set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CNOID_PLUGIN_SUBDIR}) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CNOID_PLUGIN_SUBDIR}) endif() if(ENABLE_INSTALL_RPATH) set(CMAKE_INSTALL_RPATH "$ORIGIN:$ORIGIN/..") else() set(CMAKE_INSTALL_RPATH "$ORIGIN") endif() add_subdirectory(BodyPlugin) add_subdirectory(PoseSeqPlugin) add_subdirectory(GRobotPlugin) foreach(directory CorbaPlugin MediaPlugin BalancerPlugin) if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${directory}) add_subdirectory(${directory}) endif() endforeach() choreonoid-1.1.0+dfsg/src/Choreonoid/000077500000000000000000000000001207742442300174465ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Choreonoid/CMakeLists.txt000066400000000000000000000015401207742442300222060ustar00rootroot00000000000000# @author Shin'ichiro Nakaoka set(target choreonoid) set(sources main.cpp) QT4_ADD_RESOURCES(RC_SRCS choreonoid.qrc) if(WIN32) set(sources ${sources} choreonoid.rc) endif() add_executable(${target} ${sources} ${RC_SRCS}) target_link_libraries(${target} CnoidUtil CnoidBase) set_target_properties(${target} PROPERTIES PROJECT_LABEL Application) if(MSVC) option(USE_SUBSYSTEM_CONSOLE "Attaching a console of Windows for debugging" OFF) if(NOT USE_SUBSYSTEM_CONSOLE) set_target_properties(${target} PROPERTIES WIN32_EXECUTABLE true) else() set_target_properties(${target} PROPERTIES LINK_FLAGS "/SUBSYSTEM:CONSOLE") endif() endif() apply_common_setting_for_target(${target}) if(MSVC) set_target_properties(${target} PROPERTIES DEBUG_POSTFIX -debug) endif() install(TARGETS ${target} RUNTIME DESTINATION bin CONFIGURATIONS Release Debug) choreonoid-1.1.0+dfsg/src/Choreonoid/choreonoid.desktop000066400000000000000000000004561207742442300231770ustar00rootroot00000000000000[Desktop Entry] Version=1.1 Name=Choreonoid GenericName=Integrated Robotics Platform Comment=Integrated Robotics Platform MimeType= Exec=/home/nakaoka/choreonoid/bin/choreonoid %F TryExec=choreonoid Icon=/usr/share/icons/hicolor/scalable/apps/choreonoid.svg Type=Application Terminal=false Categories= choreonoid-1.1.0+dfsg/src/Choreonoid/choreonoid.qrc000066400000000000000000000003631207742442300223100ustar00rootroot00000000000000 icon/choreonoid48.png icon/choreonoid32.png icon/choreonoid24.png icon/choreonoid16.png choreonoid-1.1.0+dfsg/src/Choreonoid/choreonoid.rc000066400000000000000000000000621207742442300221230ustar00rootroot00000000000000IDI_ICON1 ICON DISCARDABLE "icon/choreonoid.ico" choreonoid-1.1.0+dfsg/src/Choreonoid/icon/000077500000000000000000000000001207742442300203765ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Choreonoid/icon/choreonoid.ico000066400000000000000000001631041207742442300232300ustar00rootroot00000000000000 hf ˆ Î  ¨V00 ¨%þ@@ (B¦D v_Ά(  ÿÿÿXXX‚†††ø§§§úªªªûªªªû©©©û©©©û©©©û©©©û©©©û©©©û¥¥¥ø©©©ò{{{„ÿÿÿ+++ ___×ÿºººÿÃÃÃÿ¹¹¹ÿ»»»ÿ¾¾¾ÿÀÀÀÿÂÂÂÿÁÁÁÿ£££ÿ¿¿¿ÿÅÅÅô333 ^^^è555¦tttÿÿ¦¦¦ÿ¥¥¥ÿ£££ÿ¯¯¯ÿ»»»ÿÆÆÆÿÒÒÒÿÞÞÞÿÂÂÂÿ»»»ÿÐÐÐþ3336†††Ô“““ýzzzÿÌÌÌÿÿÀÀÀÿ´´´ÿ¨¨¨ÿ°°°ÿ···ÿ¾¾¾ÿÆÆÆÿ»»»ÿ¥¥¥ÿÓÓÓÿcccdqqq¨ÉÉÉÿ¥¥¥ÿmmmÿ ¤µµµîÎÎÎÿªªªÿ³³³ÿ½½½ÿÇÇÇÿÑÑÑÿÛÛÛÿÖÖÖÿÔÔÔÿƒƒƒ–JJJGÇÇÇÿºººÿ¿¿¿ÿ‰‰‰ÖLLL4Ê………ΈˆˆÎÎ’’’Η——Λ››Î   Î¢¢¢Íyyyb333 ···ù«««ÿÀÀÀÿ×××ÿÎÈÈÈoÇÇÇ)ÉÉÉ^ÈÈÈ}ÅÅʼnÇÇÇ2ÄÄÄNÄÄĆ‚ÂÂÂÿÿÿ¢¢¢èºººÿÌÌÌÿ´´´ÿ´´´ñˆˆˆgˆˆˆO‡‡‡¢†††”†††R………^………€„„„›ƒƒƒg€€€:ÿÿÿˆˆˆÌÅÅÅÿÐÐÐÿÆÆÆÿÊÊÊý???>GGGaDDDlCCCHDDDt@@@AAAOAAAJBBBŒAAASÿÿÿlllŒÖÖÖÿ½½½ÿÁÁÁÿÔÔÔÿnnnvÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ444Nÿÿÿ222)444Yÿÿÿ333-ÙÙÙþ°°°ÿÈÈÈÿÆÆÆø???-\\\™bbb¡ggg¡jjj¡ooo¡ttt¡yyy¡}}}¡pppUÿÿÿÀÀÀõµµµÿÁÁÁÿpppÿWWW圜œÿÿÿœœœÿ«««ÿºººÿÉÉÉÿÔÔÔÿ§§§Õÿÿÿÿÿÿ¦¦¦á­­­ÿ‰‰‰ÿÖÖÖÿœœœÿ³³³ÿ¦¦¦ÿ­­­ÿ³³³ÿ¸¸¸ÿ¾¾¾ÿ³³³ÿÁÁÁÿ¹¹¹äÿÿÿÿÿÿ}}}½qqqÛeeeÿwwwÿÃÃÃÿ³³³ÿ¹¹¹ÿÄÄÄÿÌÌÌÿÔÔÔÿÜÜÜÿ¸¸¸ÿÈÈÈÿÅÅÅïÿÿÿÿÿÿ@@@@@@<¬¬¬ôÌÌÌÿ°°°ÿ¯¯¯ÿµµµÿ»»»ÿÃÃÃÿÊÊÊÿ²²²ÿÐÐÐÿÓÓÓïÿÿÿÿÿÿÿÿÿÿÿÿ^^^ešššùšššû¤¤¤û¯¯¯ûºººûÆÆÆûÑÑÑûÜÜÜûáááû¾¾¾ö«€À„ƒÙƒ3ƒýƒÿÂÀÀÀøø(0 ÿÿÿÿÿÿPPPÜú›››ú›››ú›››úšššúšššúšššúšššúšššúšššúšššúšššúšššúšššúšššúšššú•••÷ñXXXmÿÿÿÿÿÿÿÿÿÿÿÿ333 gggóŽŽŽÿ»»»ÿÆÆÆÿÃÃÃÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÁÁÁÿ¿¿¿ÿ¯¯¯ÿÄÄÄÿ½½½ú333ÿÿÿ777ÿÿÿ—yyyþ‘‘‘ÿ“““ÿ»»»ÿ±±±ÿ®®®ÿ¯¯¯ÿ±±±ÿ³³³ÿµµµÿ···ÿ¸¸¸ÿºººÿµµµÿšššÿ¯¯¯ÿÄÄÄÿÚÚÚÿooo°ÿÿÿSSSðMMM¡ˆWWWÿmmmÿ‘‘‘ÿ¢¢¢ÿ¢¢¢ÿÿÿ©©©ÿ°°°ÿ¸¸¸ÿÀÀÀÿÈÈÈÿÐÐÐÿ×××ÿßßßÿÉÉÉÿ°°°ÿÄÄÄÿÑÑÑÿèÿÿÿdddÓŒŒŒýOOOúƒƒƒÿÐÐÐÿŠŠŠÿ§§§ÿ³³³ÿ³³³ÿ¤¤¤ÿ¦¦¦ÿ²²²ÿºººÿÂÂÂÿÊÊÊÿÒÒÒÿÙÙÙÿáááÿéééÿ£££ÿ­­­ÿËËËÿ¡¡¡ðÿÿÿJJJ†´´´ÿ‘‘‘ÿgggÿ¸¸¸ÿ´´´ÿaaaÿ½½½þÄÄÄÿÅÅÅÿ¥¥¥ÿ¦¦¦ÿªªªÿ­­­ÿ²²²ÿ¶¶¶ÿ¹¹¹ÿ½½½ÿÂÂÂÿ¥¥¥ÿ®®®ÿÌÌÌÿ³³³õ333222=°°°ü»»»ÿ¡¡¡ÿxxxÿbbbÿ;;;ÿbbbÉÒÒÒÿÖÖÖÿ···ÿ«««ÿ±±±ÿ¸¸¸ÿ¿¿¿ÿÅÅÅÿÌÌÌÿÓÓÓÿÙÙÙÿÞÞÞÿÊÊÊÿÍÍÍÿÆÆÆø333333 ”””õÔÔÔÿ²²²ÿ²²²ÿ’’’ü444|333xxx×¹¹¹þ¤¤¤ÿ«««ÿ±±±ÿ¸¸¸ÿ¿¿¿ÿÅÅÅÿÌÌÌÿÓÓÓÿÙÙÙÿàààÿçççÿÝÝÝÿÂÂÂû3331ÿÿÿzzzêÎÎÎÿ®®®ÿÁÁÁÿÃÃÃÿ±±±ûXXX‹333 GGGyLLLNNNNNNPPPPPPPPPRRRRRRTTTTTTTTTVVVPPP{333 ÿÿÿfff¾ËËËÿ¦¦¦ÿ§§§ÿÍÍÍÿÕÕÕÿÆÆÆý]]]…ÓÓÓFÓÓÓVÒÒÒ-ÔÔÔpÒÒÒÒÒÒ¤ÑÑѧÐÐÐ6ÏÏÏ:ÑÑÑXÐÐÐ1ÎÎθÎÎÎsÎÎÎwÿÿÿÿÿÿ???iÇÇÇÿ³³³ÿ»»»ÿ²²²ÿÒÒÒÿ¸¸¸ÿÞ©©©A¨¨¨³§§§ §§§Ó¨¨¨L§§§“¥¥¥¦¦¦Â¦¦¦H¤¤¤¢¤¤¤ƒ£££l¡¡¡.¢¢¢Åÿÿÿÿÿÿ555+´´´úÆÆÆÿ´´´ÿÖÖÖÿ²²²ÿ®®®ÿžžžówww}}}退€|||è~~~A{{{¢ÿÿÿzzzÝzzz,yyyÀxxxlyyy…xxxwwwÞÿÿÿÿÿÿ”””óÛÛÛÿ­­­ÿÖÖÖÿ¾¾¾ÿ­­­ÿÃÃÃú333(QQQÂPPPoPPPìUUUPPPNNN£NNNNNN MMM¶KKKLLL¿LLL‰KKKïÿÿÿÿÿÿÿÿÿwwwáÝÝÝÿ¯¯¯ÿÖÖÖÿ½½½ÿ¹¹¹ÿÚÚÚÿAAAdÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ÿÿÿÿÿÿ333ÿÿÿÿÿÿÿÿÿ333Ý333ÿÿÿÿÿÿ```¥ÚÚÚÿ¸¸¸ÿÁÁÁÿ¸¸¸ÿ×××ÿÛÛÛÿwww¹ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333Œ@@@ÿÿÿÿÿÿ333›333ÿÿÿÿÿÿ444OÑÑÑþÁÁÁÿ©©©ÿÄÄÄÿ×××ÿËËËúIIIL333333(333)333)333*333+333+333,333-333-333.333/333/333ÿÿÿÿÿÿ333µµµøßßßÿ±±±ÿÄÄÄÿÍÍÍÿLLLð ucccãsssùzzzúƒƒƒú‹‹‹ú“““û›››û£££û¬¬¬û´´´û½½½ûÆÆÆû²²²úIIIqÿÿÿÿÿÿÿÿÿ‘‘‘ïÓÓÓÿ±±±ÿÄÄÄÿsssÿ€€€ÿCCCÿ¡¡¡ý¦¦¦ÿ~~~ÿ†††ÿÿšššÿ¤¤¤ÿ®®®ÿ¸¸¸ÿÂÂÂÿÌÌÌÿÔÔÔÿÔÔÔÿnnn©ÿÿÿÿÿÿÿÿÿuuuÓÅÅÅÿ±±±ÿ}}}ÿÑÑÑÿ¿¿¿ÿpppÿ±±±ÿ±±±ÿŸŸŸÿŸŸŸÿ¢¢¢ÿ¤¤¤ÿ§§§ÿªªªÿ­­­ÿ°°°ÿ°°°ÿ»»»ÿÔÔÔÿÏÿÿÿÿÿÿÿÿÿSSS‡¯¯¯ÿ†††ýuuuÿ¾¾¾ÿÿ¯¯¯ÿ»»»ÿ´´´ÿ«««ÿÀÀÀÿÅÅÅÿËËËÿÐÐÐÿÕÕÕÿÛÛÛÿàààÿ¥¥¥ÿ»»»ÿÔÔÔÿŽŽŽæÿÿÿÿÿÿÿÿÿ222=wwwý%%%”GGGÿlllÿrrrÿÆÆÆÿÅÅÅÿ©©©ÿºººÿÀÀÀÿÅÅÅÿËËËÿÐÐÐÿÕÕÕÿÛÛÛÿàààÿ¬¬¬ÿÇÇÇÿ×××ÿ¡¡¡ïÿÿÿÿÿÿÿÿÿ333===7ÿÿÿ8³ºººýÐÐÐÿÌÌÌÿ   ÿ§§§ÿ©©©ÿ¬¬¬ÿ¯¯¯ÿ±±±ÿµµµÿ¸¸¸ÿ¶¶¶ÿ©©©ÿÊÊÊÿãããÿ¬¬¬ðÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿvvvÐÔÔÔÿ¹¹¹ÿ£££ÿ©©©ÿ²²²ÿºººÿÃÃÃÿËËËÿÔÔÔÿÜÜÜÿäääÿíííÿÕÕÕÿÊÊÊÿãããÿ———ãÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333xxxùŠŠŠùú———úžžžú¥¥¥ú«««ú³³³ú¹¹¹úÀÀÀúÇÇÇúÍÍÍúÔÔÔúÚÚÚú¸¸¸÷¤¤¤ñMMMDÀàà€ƒ€À€|÷ÀUMÀUUÀTQÀýÀ?Ýàÿà@àààðþþþ( @ ÿÿÿÿÿÿ333"RRRÿ}}}ûûûûûûûûûûûŒŒŒûŒŒŒûŒŒŒûŒŒŒûŒŒŒûŒŒŒûŒŒŒûŒŒŒûŒŒŒûŒŒŒû‰‰‰úŠŠŠúkkkù999_ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ???‹pppý›››ÿ»»»ÿ¹¹¹ÿ¸¸¸ÿ¸¸¸ÿ¸¸¸ÿ¸¸¸ÿ···ÿ···ÿ···ÿ···ÿ···ÿ···ÿ···ÿ···ÿ···ÿ···ÿ···ÿ···ÿºººÿ´´´ÿ···ÿÇÇÇÿ¦¦¦ü333;ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿDDD®þ†††ÿ’’’ÿ¿¿¿ÿÎÎÎÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÌÌÌÿÍÍÍÿ­­­ÿ§§§ÿ···ÿÇÇÇÿ×××ÿpppÍÿÿÿÿÿÿ000%+++ÿÿÿ!ÇFFFÿÿ“““ÿ“““ÿ“““ÿ···ÿ———ÿžžžÿ   ÿ¡¡¡ÿ¢¢¢ÿ¤¤¤ÿ¥¥¥ÿ§§§ÿ¨¨¨ÿ©©©ÿ«««ÿ¬¬¬ÿ¤¤¤ÿ˜˜˜ÿ§§§ÿ···ÿÇÇÇÿ×××ÿªªªü333&ÿÿÿNNNíGGGØ)))ÎHHHÿiiiÿYYYÿžžžÿ   ÿ   ÿ   ÿ———ÿ›››ÿ¦¦¦ÿ«««ÿ±±±ÿ···ÿ½½½ÿÃÃÃÿÉÉÉÿÎÎÎÿÔÔÔÿÚÚÚÿàààÿÓÓÓÿ¡¡¡ÿ¾¾¾ÿÉÉÉÿÑÑÑÿÏÏÏý333`ÿÿÿRRRÄ‚‚‚þOOOó444ÿoooÿ£££ÿ®®®ÿdddÿ¬¬¬ÿ­­­ÿ­­­ÿ­­­ÿ–––ÿ£££ÿ­­­ÿ³³³ÿ¸¸¸ÿ¾¾¾ÿÄÄÄÿÊÊÊÿÐÐÐÿÖÖÖÿÛÛÛÿáááÿçççÿ­­­ÿ›››ÿ±±±ÿÈÈÈÿÜÜÜÿ333yÿÿÿ666w¥¥¥þ………ÿ]]]ÿlllÿÁÁÁÿúúúÿ§§§ÿpppÿ¹¹¹ÿ¹¹¹ÿºººÿ¸¸¸ÿ———ÿ­­­ÿ´´´ÿºººÿÀÀÀÿÆÆÆÿÌÌÌÿÑÑÑÿ×××ÿÝÝÝÿãããÿéééÿ···ÿ™™™ÿ±±±ÿÈÈÈÿÞÞÞÿPPP¢ÿÿÿ111>–––û¸¸¸ÿ’’’ÿrrrÿ{{{ÿÃÃÃÿ¥¥¥ÿaaaÿ„„„ÿÆÆÆÿÆÆÆÿÇÇÇÿ¼¼¼ÿ———ÿžžžÿ   ÿ¢¢¢ÿ¤¤¤ÿ¦¦¦ÿ¨¨¨ÿ«««ÿ­­­ÿ®®®ÿ°°°ÿ   ÿ›››ÿ±±±ÿÈÈÈÿßßßÿiiiÇÿÿÿ... sssýÅÅÅÿ¢¢¢ÿŸŸŸÿŠŠŠÿ^^^ÿqqqÿJJJÿ###ߟŸŸüÓÓÓÿÓÓÓÿÍÍÍÿ§§§ÿ«««ÿ°°°ÿµµµÿ»»»ÿ¿¿¿ÿÅÅÅÿÊÊÊÿÏÏÏÿÔÔÔÿÙÙÙÿÞÞÞÿØØØÿ¾¾¾ÿÈÈÈÿßßßÿxxxáÿÿÿÿÿÿaaaèÀÀÀÿÍÍÍÿ«««ÿ¬¬¬ÿ¡¡¡ÿLLLÿÚ)???p¹¹¹üÞÞÞÿ®®®ÿ¦¦¦ÿ«««ÿ°°°ÿµµµÿ»»»ÿ¿¿¿ÿÅÅÅÿÊÊÊÿÏÏÏÿÔÔÔÿÙÙÙÿÞÞÞÿãããÿèèèÿÛÛÛÿÞÞÞÿ„„„óÿÿÿÿÿÿJJJª¾¾¾ÿÕÕÕÿ¾¾¾ÿ¸¸¸ÿ¹¹¹ÿ´´´ÿ^^^æ333ÿÿÿLLL”ŒŒŒûû“““û———û›››ûžžžû£££û¦¦¦û«««û¯¯¯û³³³û···ûºººû¾¾¾ûÂÂÂûÆÆÆûÉÉÉû¾¾¾úwwwøÿÿÿÿÿÿ444b²²²ýÒÒÒÿÿ¸¸¸ÿÅÅÅÿÅÅÅÿÅÅÅÿsssõ44463333338333@333@333@333@333@333@333@333@333@333@333@333@333@333@333@333@333@333'ÿÿÿÿÿÿ555+“““ûÏÏÏÿ£££ÿžžžÿºººÿÒÒÒÿÒÒÒÿÓÓÓÿ‡‡‡ú3332ÞÞÞÙÙÙ”ÿÿÿÛÛÛ2ØØØ{ÿÿÿÖÖÖ,××ט××׫ÖÖÖPÿÿÿÖÖÖEÕÕÕbÿÿÿÕÕÕ\ÔÔÔ¸ÕÕÕfÔÔÔ•ÎÎÎÿÿÿÿÿÿqqqûÍÍÍÿµµµÿ¬¬¬ÿ¯¯¯ÿ¶¶¶ÿßßßÿØØØÿ¸¸¸þ@@@†¶¶¶¹¹¹ÿ¶¶¶*···C¸¸¸û¶¶¶¸¸¸¨µµµŸ···@¶¶¶í³³³%´´´tµµµËÿÿÿ´´´ö³³³e¶¶¶#³³³ø³³³PÿÿÿÿÿÿÿÿÿaaaØËËËÿÏÏÏÿ¤¤¤ÿÆÆÆÿÄÄÄÿ°°°ÿ¨¨¨ÿ¿¿¿ÿkkkÔÿÿÿ˜˜˜è˜˜˜\™™™———þ•••5–––Е••eÿÿÿ•••­•••‰“““W”””è™™™“““ÿ’’’1ÿÿÿ’’’Û’’’iÿÿÿÿÿÿÿÿÿ@@@ŽÇÇÇÿÞÞÞÿ   ÿÆÆÆÿÚÚÚÿµµµÿÿ¿¿¿ÿƒƒƒúxxx´vvv¨ÿÿÿvvvÙwwwgvvv•tttˆÿÿÿttt“tttŠuuu;sssþ€€€sssÚqqqcÿÿÿrrrÃrrr‚ÿÿÿÿÿÿÿÿÿ444O³³³üÜÜÜÿ¤¤¤ÿÆÆÆÿÚÚÚÿÎÎÎÿ™™™ÿ¿¿¿ÿ¬¬¬û222)VVV|VVVÛVVVVVVæVVVnSSS.UUUßUUU{TTTáUUUWUUUSSSýNNNSSS_RRRðQQQ‹RRRÞQQQšÿÿÿÿÿÿÿÿÿ555ŽŽŽüÚÚÚÿ¬¬¬ÿÅÅÅÿÚÚÚÿáááÿŸŸŸÿ¿¿¿ÿÓÓÓý444b+++777.<<<444N... ÿÿÿ333 444Y000%ÿÿÿÿÿÿ555+ÿÿÿÿÿÿ555+111C222‰333³ÿÿÿÿÿÿÿÿÿÿÿÿpppóØØØÿÅÅÅÿ¸¸¸ÿÚÚÚÿ¬¬¬ÿÆÆÆÿÍÍÍÿáááÿVVV«ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333x555+ÿÿÿÿÿÿÿÿÿ333r333Ìÿÿÿÿÿÿÿÿÿÿÿÿ[[[ÃÕÕÕÿßßßÿ¬¬¬ÿ³³³ÿ´´´ÿÐÐÐÿÞÞÞÿàààÿwwwæÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333j1114ÿÿÿÿÿÿÿÿÿ3332444žÿÿÿÿÿÿÿÿÿÿÿÿ777uÎÎÎÿêêêÿ–––ÿ¦¦¦ÿÁÁÁÿÐÐÐÿÞÞÞÿÑÑÑýEEEˆÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333<±±±ûçççÿºººÿ³³³ÿÁÁÁÿÐÐÐÿ×××ÿLLLë:333*MMMêQQQóTTTóWWWó[[[ô^^^ôaaaõdddõgggõkkkõmmmõqqqõtttõwwwözzzö~~~÷iiië333ÿÿÿÿÿÿÿÿÿ333 †††ýåååÿ¾¾¾ÿ³³³ÿÁÁÁÿÐÐÐÿgggÿJJJÿ+++üFFFÒšššÿ|||ÿzzzÿ‚‚‚ÿ‰‰‰ÿ‘‘‘ÿ˜˜˜ÿ   ÿ§§§ÿ¯¯¯ÿ¶¶¶ÿ¾¾¾ÿÅÅÅÿÍÍÍÿÔÔÔÿ×××ÿ¾¾¾û333Dÿÿÿÿÿÿÿÿÿÿÿÿnnnçâââÿµµµÿ³³³ÿÁÁÁÿ}}}ÿ‰‰‰ÿ„„„ÿRRRÿ{{{ÿ¨¨¨ÿ§§§ÿ|||ÿ‚‚‚ÿ‰‰‰ÿ‘‘‘ÿ˜˜˜ÿ   ÿ§§§ÿ¯¯¯ÿ¶¶¶ÿ¾¾¾ÿÅÅÅÿÍÍÍÿÐÐÐÿÊÊÊÿÐÐÐþ333bÿÿÿÿÿÿÿÿÿÿÿÿRRRªàààÿ¬¬¬ÿ³³³ÿ’’’ÿÿØØØÿ±±±ÿZZZÿ®®®ÿ°°°ÿ°°°ÿŸŸŸÿÿ’’’ÿ•••ÿ———ÿ˜˜˜ÿ›››ÿÿŸŸŸÿ¢¢¢ÿ¤¤¤ÿ¶¶¶ÿ···ÿÊÊÊÿÜÜÜÿ:::‚ÿÿÿÿÿÿÿÿÿÿÿÿ222`ÏÏÏþ¦¦¦ÿŸŸŸÿ___ÿ½½½ÿåååÿ§§§ÿ€€€ÿ¸¸¸ÿ···ÿ¶¶¶ÿžžžÿ¾¾¾ÿÂÂÂÿÆÆÆÿÊÊÊÿÎÎÎÿÒÒÒÿÖÖÖÿÚÚÚÿÞÞÞÿÚÚÚÿšššÿ···ÿÊÊÊÿÝÝÝÿZZZ²ÿÿÿÿÿÿÿÿÿÿÿÿ222)™™™û¡¡¡þFFFÿdddÿ’’’ÿ¥¥¥ÿcccÿ¼¼¼ÿ¿¿¿ÿ¿¿¿ÿ©©©ÿ®®®ÿ¾¾¾ÿÂÂÂÿÆÆÆÿÊÊÊÿÎÎÎÿÒÒÒÿÖÖÖÿÚÚÚÿÞÞÞÿâââÿšššÿ···ÿÊÊÊÿÝÝÝÿqqqÖÿÿÿÿÿÿÿÿÿÿÿÿiiiùXXXå  888ÿYYYÿ```ÿ€€€ÿÇÇÇÿÇÇÇÿÆÆÆÿŸŸŸÿºººÿ¾¾¾ÿÂÂÂÿÆÆÆÿÊÊÊÿÎÎÎÿÒÒÒÿÖÖÖÿÚÚÚÿÞÞÞÿáááÿŸŸŸÿÇÇÇÿÒÒÒÿÛÛÛÿ~~~îÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ===O666!yß888þÉÉÉÿÎÎÎÿÎÎÎÿÂÂÂÿ’’’ÿžžžÿ   ÿ¡¡¡ÿ£££ÿ¤¤¤ÿ¦¦¦ÿ§§§ÿ©©©ÿ«««ÿ®®®ÿ   ÿ¨¨¨ÿÇÇÇÿÚÚÚÿíííÿ†††ïÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333‚‚‚úÖÖÖÿÔÔÔÿ¾¾¾ÿ§§§ÿªªªÿ±±±ÿ···ÿ½½½ÿÄÄÄÿÊÊÊÿÐÐÐÿ×××ÿÝÝÝÿãããÿêêêÿâââÿ¸¸¸ÿÇÇÇÿÚÚÚÿíííÿyyyÒÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ@@@†ÒÒÒþÀÀÀÿžžžÿžžžÿ¤¤¤ÿªªªÿ±±±ÿ···ÿ½½½ÿÄÄÄÿÊÊÊÿÐÐÐÿ×××ÿÝÝÝÿãããÿêêêÿðððÿëëëÿËËËÿÚÚÚÿæææÿUUU™ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ]]]ävvvû|||ú€€€ú………ú‰‰‰úú“““ú———ú›››ú   ú¤¤¤ú©©©ú­­­ú²²²ú¶¶¶ú»»»ú¿¿¿úÃÃÃú···ú™™™úcccÝ333ààðð €€€€`€`À?ÿÿÀçëÀ K[À Y[À IYà kaàÿùàÿýàÿýðÿÿð ðððøøøÿ€ÿÀÿ€ÿ€(0` ÿÿÿÿÿÿÿÿÿ333:::õ___ÿoooÿpppÿpppÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿoooÿnnnÿmmmÿXXXÿ888Þ333Dÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333bLLLÿzzzÿ¨¨¨ÿ®®®ÿ­­­ÿ­­­ÿ­­­ÿ­­­ÿ­­­ÿ­­­ÿ­­­ÿ­­­ÿ­­­ÿ­­­ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ¬¬¬ÿ±±±ÿ´´´ÿ¿¿¿ÿÁÁÁÿ\\\þ333sÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333YYYÿ{{{ÿÿ­­­ÿ¿¿¿ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿ»»»ÿºººÿºººÿºººÿºººÿºººÿºººÿºººÿ¾¾¾ÿ©©©ÿ´´´ÿ¿¿¿ÿÊÊÊÿÑÑÑÿcccþ3339ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333fffÿƒƒƒÿ„„„ÿ„„„ÿ   ÿÅÅÅÿËËËÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÁÁÁÿŸŸŸÿ©©©ÿ´´´ÿ¿¿¿ÿÊÊÊÿÔÔÔÿËËËÿ;;;Óÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ---åuuuÿŒŒŒÿŒŒŒÿŒŒŒÿÿ”””ÿÀÀÀÿ×××ÿÒÒÒÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÉÉÉÿÉÉÉÿÉÉÉÿËËËÿÅÅÅÿ”””ÿŸŸŸÿ©©©ÿ´´´ÿ¿¿¿ÿÊÊÊÿÔÔÔÿßßßÿuuuÿ333ÿÿÿÿÿÿ333y444TÿÿÿÿÿÿÿÿÿQî...ÿ<<<ÿ………ÿ•••ÿ•••ÿ•••ÿ•••ÿ•••ÿ•••ÿ˜˜˜ÿ‡‡‡ÿÿ‘‘‘ÿ’’’ÿ“““ÿ”””ÿ•••ÿ–––ÿ———ÿ˜˜˜ÿ™™™ÿšššÿ›››ÿœœœÿÿžžžÿŸŸŸÿŸŸŸÿÿÿ–––ÿ©©©ÿ´´´ÿ¿¿¿ÿÊÊÊÿÔÔÔÿßßßÿ¢¢¢ÿ333cÿÿÿÿÿÿ>>>ãHHHÿ222ÿÿÿ#ô999ÿSSSÿaaaÿGGGÿ“““ÿÿÿžžžÿžžžÿžžžÿÿ‰‰‰ÿ˜˜˜ÿ¢¢¢ÿ¦¦¦ÿªªªÿ®®®ÿ²²²ÿ¶¶¶ÿºººÿ¾¾¾ÿÁÁÁÿÅÅÅÿÉÉÉÿÍÍÍÿÑÑÑÿÕÕÕÿÙÙÙÿÝÝÝÿáááÿÙÙÙÿžžžÿœœœÿ´´´ÿÃÃÃÿÎÎÎÿÓÓÓÿØØØÿ¼¼¼ÿ333Šÿÿÿÿÿÿ333©yyyÿQQQÿ333¥ ·111ÿUUUÿuuuÿŽŽŽÿŠŠŠÿNNNÿ¡¡¡ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ§§§ÿ¡¡¡ÿŠŠŠÿ¡¡¡ÿ§§§ÿ«««ÿ¯¯¯ÿ³³³ÿ···ÿ»»»ÿ¿¿¿ÿÂÂÂÿÆÆÆÿÊÊÊÿÎÎÎÿÒÒÒÿÖÖÖÿÚÚÚÿÞÞÞÿâââÿåååÿâââÿ“““ÿ¤¤¤ÿ¦¦¦ÿµµµÿÄÄÄÿÓÓÓÿÐÐÐÿ333¤ÿÿÿÿÿÿ333rÿÿ```ÿ111ýAAAÿjjjÿ‘‘‘ÿ²²²ÿÄÄÄÿ›››ÿUUUÿ¬¬¬ÿ¯¯¯ÿ¯¯¯ÿ¯¯¯ÿ¯¯¯ÿ¯¯¯ÿ¢¢¢ÿÿ¨¨¨ÿ¬¬¬ÿ°°°ÿ´´´ÿ¸¸¸ÿ¼¼¼ÿÀÀÀÿÃÃÃÿÇÇÇÿËËËÿÏÏÏÿÓÓÓÿ×××ÿÛÛÛÿßßßÿãããÿæææÿêêêÿ’’’ÿ———ÿ¦¦¦ÿµµµÿÄÄÄÿÓÓÓÿßßßÿ555¿ÿÿÿÿÿÿ333>>ÿkkkÿ   ÿÊÊÊÿîîîÿÙÙÙÿ†††ÿbbbÿ¶¶¶ÿ···ÿ···ÿ···ÿ¸¸¸ÿ¸¸¸ÿÿ•••ÿ­­­ÿ±±±ÿµµµÿ¹¹¹ÿ½½½ÿÁÁÁÿÄÄÄÿÈÈÈÿÌÌÌÿÐÐÐÿÔÔÔÿØØØÿÜÜÜÿàààÿäääÿçççÿëëëÿ™™™ÿ–––ÿ¦¦¦ÿµµµÿÄÄÄÿÓÓÓÿâââÿJJJÞÿÿÿÿÿÿ999 RRRý³³³ÿ   ÿ‹‹‹ÿÿFFFÿÿÅÅÅÿâââÿÒÒÒÿ¬¬¬ÿ```ÿtttÿÀÀÀÿÀÀÀÿÀÀÀÿÀÀÀÿÀÀÀÿÀÀÀÿ”””ÿŸŸŸÿ²²²ÿ¶¶¶ÿºººÿ¾¾¾ÿÂÂÂÿÅÅÅÿÉÉÉÿÍÍÍÿÑÑÑÿÕÕÕÿÙÙÙÿÝÝÝÿáááÿåååÿéééÿìììÿ¦¦¦ÿ”””ÿ¦¦¦ÿµµµÿÄÄÄÿÓÓÓÿâââÿ[[[öÿÿÿÿÿÿÿÿÿ>>>Ô°°°ÿÄÄÄÿ”””ÿ”””ÿÿOOOÿxxxÿ¸¸¸ÿ°°°ÿ–––ÿsssÿ>>>ÿ‰‰‰ÿÈÈÈÿÈÈÈÿÉÉÉÿÉÉÉÿÉÉÉÿÆÆÆÿ‘‘‘ÿ‹‹‹ÿ‹‹‹ÿŒŒŒÿŒŒŒÿÿŽŽŽÿŽŽŽÿŽŽŽÿÿÿÿ‘‘‘ÿ‘‘‘ÿ’’’ÿ’’’ÿ’’’ÿŽŽŽÿ™™™ÿ¦¦¦ÿµµµÿÄÄÄÿÓÓÓÿâââÿlllÿ333 ÿÿÿÿÿÿ333—ÿ¿¿¿ÿ³³³ÿœœœÿœœœÿœœœÿ```ÿ[[[ÿ‡‡‡ÿuuuÿYYYÿ777ÿ,,,ô   ÿÑÑÑÿÑÑÑÿÑÑÑÿÑÑÑÿÎÎÎÿ©©©ÿ©©©ÿ¬¬¬ÿ°°°ÿ³³³ÿ¶¶¶ÿºººÿ½½½ÿÀÀÀÿÄÄÄÿÇÇÇÿÊÊÊÿÎÎÎÿÑÑÑÿÔÔÔÿØØØÿÛÛÛÿÞÞÞÿÝÝÝÿ»»»ÿµµµÿÄÄÄÿÓÓÓÿâââÿÿ333$ÿÿÿÿÿÿ222`€€€ÿ½½½ÿÎÎÎÿ¥¥¥ÿ¥¥¥ÿ¥¥¥ÿ¥¥¥ÿvvvÿBBBÿNNNÿ999ÿþG===ƶ¶¶ÿÚÚÚÿÚÚÚÿÚÚÚÿ´´´ÿ¦¦¦ÿ©©©ÿ¬¬¬ÿ°°°ÿ³³³ÿ¶¶¶ÿºººÿ½½½ÿÀÀÀÿÄÄÄÿÇÇÇÿÊÊÊÿÎÎÎÿÑÑÑÿÔÔÔÿØØØÿÛÛÛÿÞÞÞÿâââÿåååÿØØØÿÈÈÈÿÔÔÔÿãããÿ“““ÿ333>ÿÿÿÿÿÿ222)eeeÿ¼¼¼ÿËËËÿÅÅÅÿ­­­ÿ®®®ÿ®®®ÿ®®®ÿŒŒŒÿ:::ÿêOÿÿÿ333FFFÝÊÊÊÿâââÿÉÉÉÿ¢¢¢ÿ¦¦¦ÿ©©©ÿ¬¬¬ÿ°°°ÿ³³³ÿ¶¶¶ÿºººÿ½½½ÿÀÀÀÿÄÄÄÿÇÇÇÿÊÊÊÿÎÎÎÿÑÑÑÿÔÔÔÿØØØÿÛÛÛÿÞÞÞÿâââÿåååÿèèèÿèèèÿÛÛÛÿãããÿ¨¨¨ÿ333WÿÿÿÿÿÿMMMöºººÿÉÉÉÿØØØÿ¶¶¶ÿ¶¶¶ÿ¶¶¶ÿ¶¶¶ÿ···ÿ¢¢¢ÿGGGî1114ÿÿÿÿÿÿ333PPPíÃÃÃþžžžÿ¢¢¢ÿ¦¦¦ÿ©©©ÿ¬¬¬ÿ°°°ÿ³³³ÿ¶¶¶ÿºººÿ½½½ÿÀÀÀÿÄÄÄÿÇÇÇÿÊÊÊÿÎÎÎÿÑÑÑÿÔÔÔÿØØØÿÛÛÛÿÞÞÞÿâââÿåååÿèèèÿìììÿïïïÿÓÓÓÿ°°°ÿ333qÿÿÿÿÿÿÿÿÿ888¾µµµÿÈÈÈÿ×××ÿÄÄÄÿ¼¼¼ÿ¿¿¿ÿ¿¿¿ÿ¿¿¿ÿ¿¿¿ÿµµµÿTTTú444Sÿÿÿÿÿÿ3331@@@ÖEEEçEEEçFFFçFFFèFFFèHHHèHHHéHHHéIIIéIIIéJJJêKKKêKKKêMMMëMMMëNNNëNNNëOOOìPPPìPPPìRRRíRRRíSSSíTTTîUUUîUUUîOOOç333-ÿÿÿÿÿÿÿÿÿ444…™™™ÿÆÆÆÿÕÕÕÿ˜˜˜ÿŽŽŽÿÀÀÀÿÇÇÇÿÇÇÇÿÈÈÈÿÈÈÈÿÅÅÅÿhhhþ333xÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ444N|||ÿÄÄÄÿÔÔÔÿ°°°ÿ’’’ÿÿÂÂÂÿÐÐÐÿÐÐÐÿÐÐÐÿÐÐÐÿÑÑÑÿ„„„ÿ555—ÿÿÿÿÿÿßßßnàààjÿÿÿÿÿÿÞÞÞMÞÞÞƒæææ ÿÿÿÿÿÿÞÞÞTÜÜÜ¢ÜÜܯÜÜÜ}ßßßÿÿÿÿÿÿÚÚÚYÛÛÛwÿÿÿÿÿÿÞÞÞÚÚÚ}ÛÛÛ¯ÙÙÙjÙÙÙ6ØØØŠÛÛÛÿÿÿÿÿÿÿÿÿÿÿÿ777___ÿÃÃÃÿÒÒÒÿÇÇÇÿ•••ÿ¯¯¯ÿ’’’ÿ¿¿¿ÿØØØÿÙÙÙÿÙÙÙÿÙÙÙÿÂÂÂÿcccÿ...ÿÿÿÉÉÉÏÉÉÉþÅÅÅÿÿÿÈÈÈÈÈÈÿÉÉÉUÿÿÿÆÆÆgÇÇÇÿÇÇÇÑÈÈÈÆÆÆïÆÆÆäÉÉÉÿÿÿÅÅžÅÅÅÿÄÄÄÿÿÿÅÅųÄÄÄÿÅÅÅ™ÃÃÑÃÃÃåÃÃÃÿÂÂÂmÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿFFFèÁÁÁÿÐÐÐÿÙÙÙÿŽŽŽÿµµµÿ¿¿¿ÿšššÿ···ÿáááÿáááÿÓÓÓÿ»»»ÿ‰‰‰ÿ444Nÿÿÿ´´´³³³ÿ²²²Lÿÿÿ³³³[²²²ÿ²²²‹ÿÿÿ²²²Ç±±±ÿ³³³ÿÿÿ²²²S°°°ÿ¯¯¯ÿÿÿ±±±£¯¯¯ÿ®®®<ªªª¯¯¯þ®®®Öÿÿÿÿÿÿ¯¯¯o­­­ÿ­­­†ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ªµµµÿÏÏÏÿÞÞÞÿÿ´´´ÿÂÂÂÿÏÏÏÿ©©©ÿ¬¬¬ÿÏÏÏÿ¥¥¥ÿºººÿ¬¬¬ÿ333†ÿÿÿžžžižžžÿœœœÿÿÿššš&œœœÿ›››ÀÿÿÿœœœÐœœœÿ™™™ÿÿÿœœœ›››ÿ™™™Øÿÿÿ›››‡šššÿšššX™™™™™™ÿ™™™Êÿÿÿÿÿÿ˜˜˜H˜˜˜ÿ˜˜˜žÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333s”””ÿÍÍÍÿÜÜÜÿ¢¢¢ÿ«««ÿÂÂÂÿÐÐÐÿÝÝÝÿ²²²ÿ‹‹‹ÿ£££ÿºººÿÌÌÌÿ999Áÿÿÿ‡‡‡5ˆˆˆÿ†††¸ÿÿÿÿÿÿˆˆˆï‡‡‡ó€€€†††¸†††ÿƒƒƒ%ÿÿÿÿÿÿ†††é………ÿÿÿÿ„„„j„„„ÿ………u€€€ ƒƒƒöƒƒƒâÿÿÿÿÿÿ€€€2‚‚‚ÿ‚‚‚¶ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333333P333P333P333Q333Q333Q333R333R333R333S333S333S333T333T333T333U333U333U333V333V333V333W333W333W333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ444O‰‰‰ÿÛÛÛÿëëëÿÙÙÙÿ¬¬¬ÿµµµÿ¿¿¿ÿÈÈÈÿÒÒÒÿÜÜÜÿsssÿðv333`NNNÿUUUÿXXXÿZZZÿ^^^ÿ```ÿcccÿgggÿjjjÿlllÿpppÿsssÿvvvÿxxxÿ{{{ÿ~~~ÿ‚‚‚ÿ………ÿˆˆˆÿ‹‹‹ÿŽŽŽÿ‘‘‘ÿ•••ÿ˜˜˜ÿŒŒŒÿ999ºÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ555gggÿÚÚÚÿéééÿÍÍÍÿ¬¬¬ÿµµµÿ¿¿¿ÿÈÈÈÿÒÒÒÿ‘‘‘ÿ>>>ÿ<<<ÿ'''ÿ É???᜜œÿ†††ÿtttÿyyyÿ~~~ÿƒƒƒÿˆˆˆÿÿ’’’ÿ———ÿœœœÿ¡¡¡ÿ¦¦¦ÿ«««ÿ°°°ÿµµµÿºººÿ¿¿¿ÿÄÄÄÿÉÉÉÿÎÎÎÿÓÓÓÿØØØÿ×××ÿÖÖÖÿNNNêÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿIIIèØØØÿçççÿÂÂÂÿ¬¬¬ÿµµµÿ¿¿¿ÿÈÈÈÿ©©©ÿIIIÿrrrÿcccÿIIIÿ---ÿnnnÿ¥¥¥ÿ¥¥¥ÿƒƒƒÿyyyÿ~~~ÿƒƒƒÿˆˆˆÿÿ’’’ÿ———ÿœœœÿ¡¡¡ÿ¦¦¦ÿ«««ÿ°°°ÿµµµÿºººÿ¿¿¿ÿÄÄÄÿÉÉÉÿÎÎÎÿÓÓÓÿ×××ÿÎÎÎÿÚÚÚÿ```ý333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ªËËËÿæææÿºººÿ¬¬¬ÿµµµÿ¿¿¿ÿ···ÿMMMÿœœœÿœœœÿ†††ÿfffÿ???ÿ¥¥¥ÿªªªÿ©©©ÿ©©©ÿ~~~ÿ~~~ÿƒƒƒÿˆˆˆÿÿ’’’ÿ———ÿœœœÿ¡¡¡ÿ¦¦¦ÿ«««ÿ°°°ÿµµµÿºººÿ¿¿¿ÿÄÄÄÿÉÉÉÿÏÏÏÿÌÌÌÿÁÁÁÿÎÎÎÿÚÚÚÿtttÿ333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333s¥¥¥ÿäääÿ±±±ÿ¬¬¬ÿµµµÿ»»»ÿSSSÿ£££ÿÍÍÍÿÂÂÂÿ¢¢¢ÿfffÿnnnÿ¯¯¯ÿ¯¯¯ÿ®®®ÿ®®®ÿ¦¦¦ÿÿ‚‚‚ÿ………ÿˆˆˆÿŒŒŒÿÿ’’’ÿ•••ÿ™™™ÿœœœÿŸŸŸÿ£££ÿ¦¦¦ÿ©©©ÿ¬¬¬ÿ¶¶¶ÿ¿¿¿ÿ´´´ÿÁÁÁÿÎÎÎÿÚÚÚÿŠŠŠÿ333<ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333<ÿãããÿ¨¨¨ÿ¬¬¬ÿµµµÿcccÿÿÌÌÌÿöööÿÛÛÛÿ°°°ÿJJJÿ¬¬¬ÿ´´´ÿ´´´ÿ³³³ÿ³³³ÿ›››ÿ™™™ÿ£££ÿ¤¤¤ÿ¤¤¤ÿ¦¦¦ÿ¨¨¨ÿ©©©ÿªªªÿ¬¬¬ÿ­­­ÿ°°°ÿ±±±ÿ³³³ÿµµµÿ¶¶¶ÿ‹‹‹ÿ¦¦¦ÿ´´´ÿÁÁÁÿÎÎÎÿÚÚÚÿžžžÿ333Zÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333 ___ýÜÜÜÿ¢¢¢ÿ¬¬¬ÿyyyÿRRRÿ›››ÿÁÁÁÿÚÚÚÿÌÌÌÿŠŠŠÿnnnÿ¹¹¹ÿ¹¹¹ÿ¹¹¹ÿ¸¸¸ÿ¶¶¶ÿÿ»»»ÿ¾¾¾ÿÁÁÁÿÄÄÄÿÆÆÆÿÉÉÉÿÌÌÌÿÏÏÏÿÑÑÑÿÔÔÔÿ×××ÿÙÙÙÿÜÜÜÿßßßÿáááÿŸŸŸÿ¡¡¡ÿ´´´ÿÁÁÁÿÎÎÎÿÚÚÚÿ³³³ÿ333xÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿAAAÕÆÆÆÿ¢¢¢ÿŒŒŒÿ777ÿaaaÿ„„„ÿ¡¡¡ÿ¯¯¯ÿ¨¨¨ÿOOOÿ²²²ÿ¾¾¾ÿ¾¾¾ÿ¾¾¾ÿ½½½ÿ¤¤¤ÿ¡¡¡ÿ¼¼¼ÿ¾¾¾ÿÁÁÁÿÄÄÄÿÆÆÆÿÉÉÉÿÌÌÌÿÏÏÏÿÑÑÑÿÔÔÔÿ×××ÿÙÙÙÿÜÜÜÿßßßÿáááÿ¯¯¯ÿœœœÿ´´´ÿÁÁÁÿÎÎÎÿÚÚÚÿÊÊÊþ333–ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ222˜šššþ•••ÿ;;;ì'''ÿHHHÿeeeÿzzzÿ„„„ÿoooÿmmmÿÃÃÃÿÃÃÃÿÃÃÃÿÃÃÃÿÁÁÁÿŽŽŽÿµµµÿ¼¼¼ÿ¾¾¾ÿÁÁÁÿÄÄÄÿÆÆÆÿÉÉÉÿÌÌÌÿÏÏÏÿÑÑÑÿÔÔÔÿ×××ÿÙÙÙÿÜÜÜÿßßßÿáááÿÀÀÀÿ™™™ÿ´´´ÿÁÁÁÿÎÎÎÿÚÚÚÿßßßÿ333´ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ222aqqqÿIIIõ111* ¿)))ÿAAAÿRRRÿYYYÿ@@@ÿ¶¶¶ÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿ±±±ÿšššÿ¹¹¹ÿ¼¼¼ÿ¾¾¾ÿÁÁÁÿÄÄÄÿÆÆÆÿÉÉÉÿÌÌÌÿÏÏÏÿÑÑÑÿÔÔÔÿ×××ÿÙÙÙÿÜÜÜÿßßßÿáááÿ³³³ÿ¥¥¥ÿÅÅÅÿÐÐÐÿ×××ÿÝÝÝÿàààÿBBB×ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ666!777á222LÿÿÿŒý)))ÿ///ÿkkkÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿÍÍÍÿ‘‘‘ÿšššÿ¤¤¤ÿ¤¤¤ÿ§§§ÿ©©©ÿªªªÿ«««ÿ­­­ÿ¯¯¯ÿ±±±ÿ³³³ÿ´´´ÿµµµÿ···ÿºººÿ»»»ÿªªªÿŠŠŠÿ²²²ÿÃÃÃÿÐÐÐÿÝÝÝÿêêêÿ÷÷÷ÿGGGæÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ#\***Ö»»»ÿÒÒÒÿÒÒÒÿÒÒÒÿÐÐÐÿÁÁÁÿŸŸŸÿ™™™ÿ›››ÿžžžÿ   ÿ¢¢¢ÿ¤¤¤ÿ§§§ÿ¨¨¨ÿ«««ÿ®®®ÿ¯¯¯ÿ²²²ÿµµµÿ···ÿºººÿ¼¼¼ÿ···ÿªªªÿ···ÿÃÃÃÿÐÐÐÿÝÝÝÿêêêÿóóóÿ666äÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333%iiiþØØØÿ×××ÿ×××ÿÅÅÅÿ¨¨¨ÿ£££ÿ§§§ÿ«««ÿ°°°ÿ´´´ÿ¸¸¸ÿ¼¼¼ÿÀÀÀÿÅÅÅÿÉÉÉÿÍÍÍÿÑÑÑÿÖÖÖÿÚÚÚÿÞÞÞÿâââÿçççÿëëëÿïïïÿØØØÿ¸¸¸ÿÃÃÃÿÐÐÐÿÝÝÝÿêêêÿãããÿ555Ìÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333½½½ÿÝÝÝÿÊÊÊÿ¥¥¥ÿšššÿŸŸŸÿ£££ÿ§§§ÿ«««ÿ°°°ÿ´´´ÿ¸¸¸ÿ¼¼¼ÿÀÀÀÿÅÅÅÿÉÉÉÿÍÍÍÿÑÑÑÿÖÖÖÿÚÚÚÿÞÞÞÿâââÿçççÿëëëÿïïïÿóóóÿàààÿÄÄÄÿÐÐÐÿÝÝÝÿêêêÿ»»»ÿ333yÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333gggüÀÀÀÿŸŸŸÿ’’’ÿ–––ÿšššÿŸŸŸÿ£££ÿ§§§ÿ«««ÿ°°°ÿ´´´ÿ¸¸¸ÿ¼¼¼ÿÀÀÀÿÅÅÅÿÉÉÉÿÍÍÍÿÑÑÑÿÖÖÖÿÚÚÚÿÞÞÞÿâââÿçççÿëëëÿïïïÿóóóÿøøøÿàààÿÏÏÏÿÝÝÝÿÛÛÛÿMMMí333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333j[[[ÿ```ÿdddÿfffÿhhhÿjjjÿmmmÿoooÿqqqÿsssÿvvvÿxxxÿzzzÿ|||ÿ~~~ÿÿƒƒƒÿ………ÿ‡‡‡ÿŠŠŠÿŒŒŒÿŽŽŽÿ‘‘‘ÿ“““ÿ•••ÿ———ÿšššÿœœœÿžžžÿ………ÿfffÿ;;;Ý333+ÿÿÿð?øüüþü8€€€€€ÀÀÀ€ÀÀÀÿÿÿÿàyÿ·àfpfàg'&sà3'&sð3'6sð372sð1333ð:8sƒðÿÿóøÿÿóøÿóóøÿÿûøÿÿÿø?ÿÿÿü8üüüþþþþþÿ ÿpÿþÿþÿüÿüÿü(@€ ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333¥FFFÿ```ÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿbbbÿaaaÿaaaÿaaaÿaaaÿaaaÿaaaÿaaaÿaaaÿ___ÿLLLÿ333ü333¿333'ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333H999ügggÿƒƒƒÿ£££ÿ¨¨¨ÿ¨¨¨ÿ¨¨¨ÿ¨¨¨ÿ¨¨¨ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ§§§ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿ¦¦¦ÿŸŸŸÿ°°°ÿ»»»ÿ²²²ÿ†††ÿ999õ333eÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333f===ÿpppÿuuuÿŸŸŸÿ¸¸¸ÿ³³³ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ±±±ÿ±±±ÿ±±±ÿ±±±ÿ±±±ÿ±±±ÿ±±±ÿ±±±ÿ±±±ÿ±±±ÿ±±±ÿºººÿ®®®ÿ³³³ÿ»»»ÿÃÃÃÿËËËÿ®®®ÿ???ÿ333Wÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333„DDDÿzzzÿ|||ÿ|||ÿÿ···ÿÀÀÀÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿÂÂÂÿ¦¦¦ÿ«««ÿ³³³ÿ»»»ÿÃÃÃÿËËËÿÓÓÓÿ¼¼¼ÿ777í333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333¢LLLÿ‚‚‚ÿ‚‚‚ÿƒƒƒÿƒƒƒÿ„„„ÿ­­­ÿÉÉÉÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÈÈÈÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÇÇÇÿÉÉÉÿŸŸŸÿ£££ÿ«««ÿ³³³ÿ»»»ÿÃÃÃÿËËËÿÓÓÓÿÛÛÛÿÿ333«ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333222¾WWWÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿœœœÿÇÇÇÿÔÔÔÿÓÓÓÿÓÓÓÿÓÓÓÿÓÓÓÿÓÓÓÿÓÓÓÿÓÓÓÿÓÓÓÿÓÓÓÿÓÓÓÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÒÒÒÿÑÑÑÿ———ÿ›››ÿ£££ÿ«««ÿ³³³ÿ»»»ÿÃÃÃÿËËËÿÓÓÓÿÛÛÛÿØØØÿ===ø333 ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿFä///ÿeeeÿÿÿÿÿÿÿÿ“““ÿ½½½ÿÚÚÚÿ¯¯¯ÿ¬¬¬ÿ¬¬¬ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ«««ÿ°°°ÿŒŒŒÿ’’’ÿ›››ÿ£££ÿ«««ÿ³³³ÿ»»»ÿÃÃÃÿËËËÿÓÓÓÿÛÛÛÿãããÿjjjÿ333Kÿÿÿÿÿÿÿÿÿ222Ž333¾///ÿÿÿÿÿÿÿÿÿÿÿÿŽÿ+++ÿ999ÿ666ÿsssÿ–––ÿ–––ÿ–––ÿ–––ÿ–––ÿ–––ÿ–––ÿ–––ÿ•••ÿ‚‚‚ÿ‡‡‡ÿŽŽŽÿÿÿ’’’ÿ“““ÿ•••ÿ–––ÿ———ÿ˜˜˜ÿšššÿ›››ÿœœœÿžžžÿŸŸŸÿ¡¡¡ÿ¢¢¢ÿ£££ÿ¥¥¥ÿ¦¦¦ÿ§§§ÿ©©©ÿªªªÿ£££ÿŠŠŠÿ„„„ÿŒŒŒÿŸŸŸÿ«««ÿ³³³ÿ»»»ÿÃÃÃÿËËËÿÓÓÓÿÛÛÛÿãããÿ’’’ÿ333ÿÿÿÿÿÿÿÿÿ333ÞOOOÿ333å3332ÿÿÿÿÿÿUÿ111ÿFFFÿWWWÿ[[[ÿ:::ÿÿœœœÿœœœÿœœœÿœœœÿÿÿÿÿ”””ÿƒƒƒÿ›››ÿ¡¡¡ÿ¤¤¤ÿ§§§ÿ©©©ÿ¬¬¬ÿ¯¯¯ÿ²²²ÿµµµÿ¸¸¸ÿ»»»ÿ¾¾¾ÿÁÁÁÿÄÄÄÿÇÇÇÿÊÊÊÿÌÌÌÿÏÏÏÿÒÒÒÿÕÕÕÿØØØÿÛÛÛÿÞÞÞÿáááÿÖÖÖÿ³³³ÿ†††ÿ¦¦¦ÿ³³³ÿ»»»ÿÃÃÃÿÎÎÎÿÕÕÕÿØØØÿÖÖÖÿ®®®ÿ333´ÿÿÿÿÿÿÿÿÿ444­kkkÿ\\\ÿ777ö222Qê---ÿGGGÿ^^^ÿrrrÿÿsssÿ<<<ÿÿ£££ÿ£££ÿ£££ÿ£££ÿ£££ÿ£££ÿ£££ÿ£££ÿ“““ÿ‰‰‰ÿ¡¡¡ÿ¤¤¤ÿ§§§ÿªªªÿ­­­ÿ°°°ÿ³³³ÿ¶¶¶ÿ¹¹¹ÿ¼¼¼ÿ¿¿¿ÿÂÂÂÿÄÄÄÿÇÇÇÿÊÊÊÿÍÍÍÿÐÐÐÿÓÓÓÿÖÖÖÿÙÙÙÿÜÜÜÿßßßÿâââÿåååÿçççÿ­­­ÿ–––ÿºººÿ¹¹¹ÿ³³³ÿ···ÿÂÂÂÿÍÍÍÿØØØÿÀÀÀÿ333Îÿÿÿÿÿÿÿÿÿ444vaaaÿ{{{ÿiiiÿ;;;þ"""µ ÿ===ÿYYYÿtttÿŒŒŒÿŸŸŸÿ©©©ÿÿ@@@ÿÿ©©©ÿ©©©ÿ©©©ÿ©©©ÿªªªÿªªªÿªªªÿªªªÿŽŽŽÿŽŽŽÿ¥¥¥ÿ¨¨¨ÿ«««ÿ®®®ÿ±±±ÿ´´´ÿ···ÿºººÿ¼¼¼ÿ¿¿¿ÿÂÂÂÿÅÅÅÿÈÈÈÿËËËÿÎÎÎÿÑÑÑÿÔÔÔÿ×××ÿÚÚÚÿÝÝÝÿßßßÿâââÿåååÿèèèÿÍÍÍÿ‹‹‹ÿ•••ÿ   ÿ«««ÿ···ÿÂÂÂÿÍÍÍÿÙÙÙÿÒÒÒÿ333éÿÿÿÿÿÿÿÿÿ555?IIIÿ¢¢¢ÿ~~~ÿuuuÿCCCÿ000ÿIIIÿgggÿ………ÿ¡¡¡ÿ¹¹¹ÿÉÉÉÿÇÇÇÿyyyÿIIIÿ¨¨¨ÿ¯¯¯ÿ°°°ÿ°°°ÿ°°°ÿ°°°ÿ°°°ÿ°°°ÿ­­­ÿ‡‡‡ÿ———ÿ©©©ÿ¬¬¬ÿ¯¯¯ÿ²²²ÿ´´´ÿ···ÿºººÿ½½½ÿÀÀÀÿÃÃÃÿÆÆÆÿÉÉÉÿÌÌÌÿÏÏÏÿÒÒÒÿÕÕÕÿ×××ÿÚÚÚÿÝÝÝÿàààÿãããÿæææÿéééÿÜÜÜÿ………ÿ•••ÿ   ÿ«««ÿ···ÿÂÂÂÿÍÍÍÿÙÙÙÿâââÿ555þ333ÿÿÿÿÿÿ... 555ü¨¨¨ÿ¯¯¯ÿ‚‚‚ÿ€€€ÿNNNÿ:::ÿlllÿÿ¯¯¯ÿÍÍÍÿçççÿãããÿÈÈÈÿeeeÿUUUÿ³³³ÿ¶¶¶ÿ¶¶¶ÿ¶¶¶ÿ¶¶¶ÿ¶¶¶ÿ···ÿ···ÿ®®®ÿ„„„ÿ   ÿ­­­ÿ¯¯¯ÿ²²²ÿµµµÿ¸¸¸ÿ»»»ÿ¾¾¾ÿÁÁÁÿÄÄÄÿÇÇÇÿÊÊÊÿÍÍÍÿÐÐÐÿÒÒÒÿÕÕÕÿØØØÿÛÛÛÿÞÞÞÿáááÿäääÿçççÿêêêÿêêêÿÿ”””ÿ   ÿ¬¬¬ÿ···ÿÂÂÂÿÍÍÍÿÙÙÙÿäääÿFFFÿ333ÿÿÿÿÿÿÿÿÿ333Ñÿ¶¶¶ÿŽŽŽÿˆˆˆÿ‡‡‡ÿ\\\ÿ<<<ÿƒƒƒÿ°°°ÿÐÐÐÿîîîÿéééÿÊÊÊÿªªªÿOOOÿdddÿ»»»ÿ¼¼¼ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ©©©ÿ………ÿ¨¨¨ÿ°°°ÿ³³³ÿ¶¶¶ÿ¹¹¹ÿ¼¼¼ÿ¿¿¿ÿÂÂÂÿÅÅÅÿÈÈÈÿÊÊÊÿÍÍÍÿÐÐÐÿÓÓÓÿÖÖÖÿÙÙÙÿÜÜÜÿßßßÿâââÿåååÿèèèÿëëëÿíííÿ‹‹‹ÿ‘‘‘ÿ   ÿ¬¬¬ÿ···ÿÂÂÂÿÍÍÍÿÙÙÙÿäääÿZZZÿ3338ÿÿÿÿÿÿÿÿÿ333šuuuÿ´´´ÿ½½½ÿŽŽŽÿŽŽŽÿÿmmmÿ;;;ÿ‰‰‰ÿÀÀÀÿÑÑÑÿÏÏÏÿ»»»ÿ¡¡¡ÿ€€€ÿ>>>ÿvvvÿÃÃÃÿÃÃÃÿÃÃÃÿÃÃÃÿÃÃÃÿÃÃÃÿÃÃÃÿÄÄÄÿ   ÿŒŒŒÿ¨¨¨ÿªªªÿ­­­ÿ¯¯¯ÿ²²²ÿ´´´ÿ¶¶¶ÿ¸¸¸ÿºººÿ½½½ÿ¿¿¿ÿÂÂÂÿÄÄÄÿÇÇÇÿÉÉÉÿËËËÿÎÎÎÿÐÐÐÿÒÒÒÿÔÔÔÿ×××ÿÙÙÙÿ”””ÿŽŽŽÿ¡¡¡ÿ¬¬¬ÿ···ÿÂÂÂÿÎÎÎÿÙÙÙÿäääÿmmmÿ333Rÿÿÿÿÿÿÿÿÿ444c[[[ÿ³³³ÿÀÀÀÿÿ•••ÿ•••ÿ•••ÿ~~~ÿ===ÿzzzÿ²²²ÿ°°°ÿ£££ÿŽŽŽÿtttÿVVVÿ444ÿŠŠŠÿÉÉÉÿÉÉÉÿÉÉÉÿÊÊÊÿÊÊÊÿÊÊÊÿÊÊÊÿÉÉÉÿ™™™ÿ‡‡‡ÿ‡‡‡ÿ‡‡‡ÿ‡‡‡ÿ‡‡‡ÿ‡‡‡ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿŠŠŠÿ‹‹‹ÿ‹‹‹ÿ‹‹‹ÿ‹‹‹ÿ‹‹‹ÿ‹‹‹ÿ‹‹‹ÿ‹‹‹ÿÿ™™™ÿ¡¡¡ÿ¬¬¬ÿ···ÿÂÂÂÿÎÎÎÿÙÙÙÿäääÿ€€€ÿ333lÿÿÿÿÿÿÿÿÿ444,BBBÿ²²²ÿ½½½ÿÇÇÇÿ›››ÿ›››ÿ›››ÿœœœÿŽŽŽÿDDDÿ^^^ÿÿ‡‡‡ÿvvvÿ```ÿHHHÿ...ÿ333ý   ÿÐÐÐÿÐÐÐÿÐÐÐÿÐÐÐÿÐÐÐÿÐÐÐÿÎÎÎÿ«««ÿ¨¨¨ÿªªªÿ­­­ÿ¯¯¯ÿ²²²ÿ´´´ÿ···ÿ¹¹¹ÿ¼¼¼ÿ¾¾¾ÿÁÁÁÿÃÃÃÿÆÆÆÿÈÈÈÿËËËÿÍÍÍÿÐÐÐÿÒÒÒÿÕÕÕÿ×××ÿÚÚÚÿÜÜÜÿßßßÿÝÝÝÿ¾¾¾ÿ¬¬¬ÿ···ÿÃÃÃÿÎÎÎÿÙÙÙÿäääÿ”””ÿ333†ÿÿÿÿÿÿÿÿÿ333󦦦ÿ¼¼¼ÿÈÈÈÿ­­­ÿ¢¢¢ÿ¢¢¢ÿ¢¢¢ÿ¢¢¢ÿ›››ÿQQQÿFFFÿfffÿ[[[ÿIIIÿ333ÿÿy999õ²²²ÿÖÖÖÿÖÖÖÿÖÖÖÿ×××ÿ×××ÿ¹¹¹ÿ¥¥¥ÿ¨¨¨ÿªªªÿ­­­ÿ¯¯¯ÿ²²²ÿ´´´ÿ···ÿ¹¹¹ÿ¼¼¼ÿ¾¾¾ÿÁÁÁÿÃÃÃÿÆÆÆÿÈÈÈÿËËËÿÍÍÍÿÐÐÐÿÒÒÒÿÕÕÕÿ×××ÿÚÚÚÿÜÜÜÿßßßÿáááÿäääÿØØØÿ¾¾¾ÿÃÃÃÿÎÎÎÿÙÙÙÿäääÿ¨¨¨ÿ333¡ÿÿÿÿÿÿÿÿÿÿÿÿ333¿‹‹‹ÿ»»»ÿÆÆÆÿÐÐÐÿ¨¨¨ÿ¨¨¨ÿ¨¨¨ÿ¨¨¨ÿ©©©ÿ§§§ÿcccÿ777ÿ>>>ÿ///ÿÿ¤ÿÿÿ333M@@@üÄÄÄÿÝÝÝÿÝÝÝÿÝÝÝÿÌÌÌÿ£££ÿ¥¥¥ÿ¨¨¨ÿªªªÿ­­­ÿ¯¯¯ÿ²²²ÿ´´´ÿ···ÿ¹¹¹ÿ¼¼¼ÿ¾¾¾ÿÁÁÁÿÃÃÃÿÆÆÆÿÈÈÈÿËËËÿÍÍÍÿÐÐÐÿÒÒÒÿÕÕÕÿ×××ÿÚÚÚÿÜÜÜÿßßßÿáááÿäääÿæææÿçççÿÔÔÔÿÎÎÎÿÙÙÙÿäääÿ»»»ÿ333»ÿÿÿÿÿÿÿÿÿÿÿÿ333ˆoooÿºººÿÅÅÅÿÑÑÑÿ»»»ÿ®®®ÿ¯¯¯ÿ¯¯¯ÿ¯¯¯ÿ¯¯¯ÿ¯¯¯ÿxxxÿ222ÿ÷lÿÿÿÿÿÿ333hKKKÿÔÔÔÿãããÿÞÞÞÿ§§§ÿ£££ÿ¥¥¥ÿ¨¨¨ÿªªªÿ­­­ÿ¯¯¯ÿ²²²ÿ´´´ÿ···ÿ¹¹¹ÿ¼¼¼ÿ¾¾¾ÿÁÁÁÿÃÃÃÿÆÆÆÿÈÈÈÿËËËÿÍÍÍÿÐÐÐÿÒÒÒÿÕÕÕÿ×××ÿÚÚÚÿÜÜÜÿßßßÿáááÿäääÿæææÿéééÿëëëÿæææÿÛÛÛÿåååÿÏÏÏÿ333Õÿÿÿÿÿÿÿÿÿÿÿÿ222QTTTÿ¸¸¸ÿÄÄÄÿÏÏÏÿØØØÿµµµÿµµµÿµµµÿµµµÿµµµÿµµµÿ¶¶¶ÿÿ:::ö444Sÿÿÿÿÿÿÿÿÿÿÿÿ333†XXXÿãããÿ¡¡¡ÿ   ÿ£££ÿ¥¥¥ÿ¨¨¨ÿªªªÿ­­­ÿ¯¯¯ÿ²²²ÿ´´´ÿ···ÿ¹¹¹ÿ¼¼¼ÿ¾¾¾ÿÁÁÁÿÃÃÃÿÆÆÆÿÈÈÈÿËËËÿÍÍÍÿÐÐÐÿÒÒÒÿÕÕÕÿ×××ÿÚÚÚÿÜÜÜÿßßßÿáááÿäääÿæææÿéééÿëëëÿîîîÿìììÿÐÐÐÿßßßÿ333ðÿÿÿÿÿÿÿÿÿÿÿÿ111;;;ÿ¶¶¶ÿÃÃÃÿÎÎÎÿÙÙÙÿÊÊÊÿ»»»ÿ»»»ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ¼¼¼ÿ£££ÿBBBþ333yÿÿÿÿÿÿÿÿÿÿÿÿ333£WWWÿeeeÿfffÿhhhÿiiiÿkkkÿlllÿnnnÿnnnÿpppÿrrrÿsssÿuuuÿvvvÿxxxÿyyyÿzzzÿ}}}ÿ~~~ÿÿ€€€ÿ‚‚‚ÿƒƒƒÿ†††ÿ‡‡‡ÿˆˆˆÿŠŠŠÿŒŒŒÿÿÿÿ’’’ÿ“““ÿ•••ÿ–––ÿ™™™ÿŒŒŒÿ444ñ333ÿÿÿÿÿÿÿÿÿÿÿÿ333ã¡¡¡ÿÁÁÁÿÍÍÍÿØØØÿµµµÿŸŸŸÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿÂÂÂÿ¶¶¶ÿQQQÿ333 +++ÿÿÿÿÿÿ333333^333x333x333y333y333z333z333z333{333{333|333|333}333}333~333~333~333333333€333€333333333‚333‚333‚333ƒ333ƒ333„333„333…333…333†333†333†333‡333‡333Iÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ222¬………ÿÀÀÀÿËËËÿ×××ÿ¨¨¨ÿ€€€ÿ———ÿÅÅÅÿÈÈÈÿÈÈÈÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÄÄÄÿdddÿ444Á666ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ222uhhhÿ¿¿¿ÿÊÊÊÿÖÖÖÿ¿¿¿ÿ‰‰‰ÿÿÿÇÇÇÿÏÏÏÿÏÏÏÿÏÏÏÿÏÏÏÿÏÏÏÿÏÏÏÿÏÏÏÿÿ333Ü333#ÿÿÿÿÿÿßßß áááfããã6ÿÿÿÿÿÿÿÿÿâââ>àààbàààÿÿÿÿÿÿÿÿÿÿÿÿààà[ààà‹ßßß™àààrÞÞÞ'ÿÿÿÿÿÿÿÿÿÿÿÿÞÞÞNÞÞÞ\èèè ÿÿÿÿÿÿÿÿÿÛÛÛ#ÝÝÝxÝÝÝ”ÜÜÜWÿÿÿÜÜÜIÜÜÜ_áááÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ555?MMMÿ¾¾¾ÿÉÉÉÿÔÔÔÿÒÒÒÿƒƒƒÿ¬¬¬ÿ™™™ÿ‰‰‰ÿÅÅÅÿÕÕÕÿÕÕÕÿÕÕÕÿÖÖÖÿÖÖÖÿÖÖÖÿÖÖÖÿ~~~ÿ444­ÿÿÿÿÿÿÑÑÑzÑÑÑÿÑÑÑóÌÌÌ ÿÿÿÿÿÿÐÐÐÌÐÐÐÿÏÏϦÿÿÿÿÿÿÕÕÕ ÏÏÏÅÏÏÏÿÏÏÏÿÏÏÏÿÏÏÏÿÎÎÎúÍÍÍ€ÿÿÿÿÿÿÿÿÿÍÍÍþÍÍÍÿÎÎÎbÿÿÿÿÿÿÌÌÌKÌÌÌÿÌÌÌÿÌÌÌûÌÌÌüËËË·ËËËøËËËÿËËËzÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ... 555ü¹¹¹ÿÈÈÈÿÓÓÓÿÞÞÞÿ………ÿ¦¦¦ÿ···ÿ§§§ÿ………ÿ¿¿¿ÿÜÜÜÿÜÜÜÿÜÜÜÿÜÜÜÿÜÜÜÿÉÉÉÿ´´´ÿ333éÿÿÿÿÿÿÁÁÁRÁÁÁÿÁÁÁÿÀÀÀ=ÿÿÿÿÿÿÀÀÀ¢ÀÀÀÿÀÀÀæÿÿÿÿÿÿ¿¿¿l¿¿¿ÿÀÀÀý¿¿¿X½½½½½½m¾¾¾þ¾¾¾ÿ½½½Mÿÿÿÿÿÿ½½½ï½½½ÿ½½½ˆÿÿÿÿÿÿ¼¼¼ß¼¼¼ÿ½½½ÀÃÃû»»ºººs»»»ÿ»»»ÿºººœÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ÑœœœÿÇÇÇÿÒÒÒÿÝÝÝÿ›››ÿœœœÿ···ÿÁÁÁÿ···ÿ†††ÿµµµÿâââÿâââÿâââÿÉÉÉÿ···ÿÈÈÈÿBBBÿ666!ÿÿÿ³³³±±±ÿ±±±ÿ¯¯¯pÿÿÿÿÿÿ°°°n°°°ÿ¯¯¯ÿ³³³ÿÿÿ°°°·¯¯¯ÿ¯¯¯Æÿÿÿÿÿÿÿÿÿ®®®®®®®ÿ­­­Óÿÿÿÿÿÿ¬¬¬Ò­­­ÿ­­­¥ÿÿÿ¶¶¶¬¬¬ÿ¬¬¬ÿ«««gÿÿÿÿÿÿÿÿÿ«««Ú«««ÿ«««³ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333šÿÆÆÆÿÑÑÑÿÜÜÜÿ³³³ÿ”””ÿ···ÿÁÁÁÿËËËÿÈÈÈÿÿ¨¨¨ÿçççÿÀÀÀÿ¦¦¦ÿ···ÿÈÈÈÿfffÿ444Xÿÿÿÿÿÿ   ê   ÿ   ¤ÿÿÿÿÿÿžžž:ŸŸŸÿŸŸŸÿŸŸŸPÿÿÿŸŸŸ¼žžžÿžžžÇÿÿÿÿÿÿÿÿÿžžžtÿÿŸŸŸÿÿÿ¶œœœÿœœœÁÿÿÿ›››œœœÿ›››ÿ›››cÿÿÿÿÿÿÿÿÿ›››¼šššÿ›››Ëÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ444caaaÿÄÄÄÿÐÐÐÿÛÛÛÿÉÉÉÿ‹‹‹ÿ···ÿÁÁÁÿËËËÿÕÕÕÿ×××ÿ˜˜˜ÿ–––ÿ•••ÿ¦¦¦ÿ···ÿÈÈÈÿ‹‹‹ÿ444ÿÿÿÿÿÿ¶ÿ×ÿÿÿÿÿÿ™™™ üÿ…ÿÿÿŽŽŽ·ŽŽŽÿŽŽŽÏÿÿÿÿÿÿÿÿÿŽŽŽ?ÿÿŒŒŒPÿÿÿŒŒŒ™ŒŒŒÿŒŒŒÞÿÿÿ‰‰‰‹‹‹ÿ‹‹‹ÿŒŒŒhÿÿÿÿÿÿÿÿÿŠŠŠ¥ŠŠŠÿŠŠŠâÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ444,DDDÿÃÃÃÿÎÎÎÿÚÚÚÿÞÞÞÿÿµµµÿÁÁÁÿËËËÿÕÕÕÿÞÞÞÿÙÙÙÿ‚‚‚ÿ•••ÿ¦¦¦ÿ···ÿÈÈÈÿ°°°ÿ333Çÿÿÿÿÿÿ€€€‚ÿý€€€ÿÿÿÿÿÿ€€€Ò~~~ÿ~~~ºÿÿÿ~~~„~~~ÿ~~~õ€€€ÿÿÿÿÿÿ}}}-}}}ÿ|||ÿ|||Tÿÿÿ|||}|||ÿ|||ùÿÿÿ{{{Ø{{{ÿ{{{¨ÿÿÿÿÿÿÿÿÿzzzŽzzzÿzzzùÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333óµµµÿÍÍÍÿÙÙÙÿäääÿÿªªªÿÁÁÁÿËËËÿÕÕÕÿÞÞÞÿèèèÿ‡‡‡ÿ‘‘‘ÿ¦¦¦ÿ···ÿÈÈÈÿÒÒÒÿ555ø+++ÿÿÿoooNoooÿoooÿooo–ÿÿÿÿÿÿmmm¡nnnÿnnnëÿÿÿnnnHmmmÿmmmÿmmm1ÿÿÿÿÿÿmmm*lllÿlllÿkkkOÿÿÿmmm`kkkÿkkkÿoooÿÿÿkkk†kkkÿjjjîUUUÿÿÿÿÿÿiiiwiiiÿiiiÿiiiÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333¾–––ÿÌÌÌÿ×××ÿãããÿ¤¤¤ÿžžžÿÁÁÁÿËËËÿÕÕÕÿÞÞÞÿèèèÿ   ÿŒŒŒÿ¦¦¦ÿ···ÿÈÈÈÿÙÙÙÿUUUÿ4446ÿÿÿbbb___ÿ___ÿ___ã^^^O^^^˜^^^ÿ^^^éÿÿÿ€€€]]]Ê]]]ÿ^^^›ÿÿÿÿÿÿ[[[Z\\\ÿ\\\ùYYYÿÿÿZZZD[[[ÿ[[[ÿZZZ3ÿÿÿ]]],ZZZùZZZÿZZZ“fffUUUYYY•YYYÿYYYÿYYY(ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333‡wwwÿËËËÿÖÖÖÿáááÿ¾¾¾ÿ”””ÿÁÁÁÿËËËÿÕÕÕÿÞÞÞÿèèèÿ»»»ÿÿ¦¦¦ÿ···ÿÈÈÈÿÙÙÙÿ}}}ÿ333mÿÿÿÿÿÿOOOâNNNÿNNNNNNÁNNNæNNNÿNNNÿNNNÙÿÿÿÿÿÿNNN.MMMæLLLÿLLLØLLL¿LLLøLLLÿKKK“ÿÿÿÿÿÿHHH KKKÿKKKÿIIIIÿÿÿÿÿÿJJJYJJJþIIIÿIIIóIIIèIIIîIIIÿIIIÿHHH@ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ222QXXXÿÊÊÊÿÕÕÕÿàààÿÕÕÕÿˆˆˆÿÁÁÁÿËËËÿÕÕÕÿÞÞÞÿèèèÿÔÔÔÿŠŠŠÿ¦¦¦ÿ···ÿÈÈÈÿÙÙÙÿ¥¥¥ÿ333¤ÿÿÿÿÿÿ???Q>>>­===K+++<<>>²<<OOOÿÏÏÏÿÚÚÚÿæææÿáááÿ………ÿËËËÿºººÿ‡‡‡ÿ···ÿÅÅÅÿÌÌÌÿÓÓÓÿÚÚÚÿâââÿéééÿîîîÿ¡¡¡ÿ333©ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ$$$5555777ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333F444­222[ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ... 555üÉÉÉÿÙÙÙÿåååÿðððÿ†††ÿ³³³ÿ………ÿ©©©ÿ¾¾¾ÿÅÅÅÿÌÌÌÿÓÓÓÿÚÚÚÿâââÿéééÿäääÿNNNÿ333Uÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ЪªªÿØØØÿãããÿïïïÿŸŸŸÿƒƒƒÿ›››ÿ···ÿ¾¾¾ÿÅÅÅÿÌÌÌÿÓÓÓÿÚÚÚÿâââÿèèèÿiiiÿ333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333š‰‰‰ÿ×××ÿâââÿíííÿÑÑÑÿ–––ÿ¯¯¯ÿ···ÿ¾¾¾ÿÅÅÅÿÌÌÌÿÓÓÓÿÚÚÚÿâââÿ‰‰‰ÿ000É@@@ÿÿÿÿÿÿ333333¦333È333È333È333È333É333É333É333É333Ê333Ê333Ê333Ê333Ë333Ë333Ë333Ë333Ì333Ì333Ì333Ì333Í333Í333Í333Í333Î333Î333Î333Î333Ï333Ï333Ï333Ï333›333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ444cgggÿÖÖÖÿáááÿìììÿïïïÿ¨¨¨ÿ¯¯¯ÿ···ÿ¾¾¾ÿÅÅÅÿÌÌÌÿÓÓÓÿÚÚÚÿ¨¨¨ÿ111ÿÿ ¼ÿÿÿ333”XXXÿ```ÿdddÿgggÿiiiÿmmmÿpppÿsssÿuuuÿyyyÿ|||ÿÿƒƒƒÿ………ÿˆˆˆÿ‹‹‹ÿŽŽŽÿ‘‘‘ÿ”””ÿ˜˜˜ÿ›››ÿžžžÿ¡¡¡ÿ¤¤¤ÿ¨¨¨ÿ«««ÿ®®®ÿ±±±ÿ´´´ÿ¸¸¸ÿ»»»ÿ¾¾¾ÿ­­­ÿdddÿ333\ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ444,FFFÿÕÕÕÿàààÿëëëÿâââÿ¨¨¨ÿ¯¯¯ÿ···ÿ¾¾¾ÿÅÅÅÿÌÌÌÿÓÓÓÿ½½½ÿ<<<ÿ>>>ÿ555ÿ&&&ÿò g999÷˜˜˜ÿ’’’ÿqqqÿuuuÿxxxÿ|||ÿ€€€ÿ„„„ÿ‡‡‡ÿ‹‹‹ÿÿ“““ÿ———ÿšššÿžžžÿ¢¢¢ÿ¦¦¦ÿ©©©ÿ­­­ÿ±±±ÿµµµÿ¸¸¸ÿ¼¼¼ÿÀÀÀÿÄÄÄÿÇÇÇÿËËËÿÏÏÏÿÓÓÓÿÖÖÖÿÚÚÚÿØØØÿÔÔÔÿ„„„ÿ333{ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333òÅÅÅÿßßßÿêêêÿÖÖÖÿ¨¨¨ÿ¯¯¯ÿ···ÿ¾¾¾ÿÅÅÅÿÌÌÌÿÉÉÉÿJJJÿTTTÿ___ÿRRRÿ@@@ÿ+++ÿ%%%úgggÿ£££ÿ£££ÿ•••ÿuuuÿxxxÿ|||ÿ€€€ÿ„„„ÿ‡‡‡ÿ‹‹‹ÿÿ“““ÿ———ÿšššÿžžžÿ¢¢¢ÿ¦¦¦ÿ©©©ÿ­­­ÿ±±±ÿµµµÿ¸¸¸ÿ¼¼¼ÿÀÀÀÿÄÄÄÿÇÇÇÿËËËÿÏÏÏÿÓÓÓÿÖÖÖÿÚÚÚÿÐÐÐÿÙÙÙÿšššÿ333šÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333¾¢¢¢ÿÝÝÝÿéééÿÌÌÌÿ¨¨¨ÿ¯¯¯ÿ···ÿ¾¾¾ÿÅÅÅÿËËËÿ```ÿYYYÿ†††ÿ}}}ÿmmmÿYYYÿ@@@ÿ777ÿÿ§§§ÿ¦¦¦ÿ¦¦¦ÿ‹‹‹ÿxxxÿ|||ÿ€€€ÿ„„„ÿ‡‡‡ÿ‹‹‹ÿÿ“““ÿ———ÿšššÿžžžÿ¢¢¢ÿ¦¦¦ÿ©©©ÿ­­­ÿ±±±ÿµµµÿ¸¸¸ÿ¼¼¼ÿÀÀÀÿÄÄÄÿÇÇÇÿËËËÿÏÏÏÿÓÓÓÿÓÓÓÿÆÆÆÿÏÏÏÿÙÙÙÿ¯¯¯ÿ333¸ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333‡ÿÜÜÜÿèèèÿÂÂÂÿ¨¨¨ÿ¯¯¯ÿ···ÿ¾¾¾ÿÅÅÅÿzzzÿPPPÿ¦¦¦ÿ¦¦¦ÿšššÿ†††ÿnnnÿCCCÿeeeÿªªªÿªªªÿªªªÿªªªÿªªªÿÿ|||ÿ€€€ÿ„„„ÿ‡‡‡ÿ‹‹‹ÿÿ“““ÿ———ÿšššÿžžžÿ¢¢¢ÿ¦¦¦ÿ©©©ÿ­­­ÿ±±±ÿµµµÿ¸¸¸ÿ¼¼¼ÿÀÀÀÿÄÄÄÿÇÇÇÿËËËÿÏÏÏÿÉÉÉÿ¼¼¼ÿÆÆÆÿÏÏÏÿÙÙÙÿÃÃÃÿ333Öÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333P\\\ÿÛÛÛÿæææÿ¸¸¸ÿ¨¨¨ÿ¯¯¯ÿ···ÿ¾¾¾ÿ”””ÿBBBÿ¯¯¯ÿÇÇÇÿÅÅÅÿ´´´ÿ›››ÿ{{{ÿ888ÿ   ÿ®®®ÿ®®®ÿ®®®ÿ®®®ÿ®®®ÿ«««ÿ~~~ÿ€€€ÿƒƒƒÿ†††ÿŠŠŠÿÿ‘‘‘ÿ”””ÿ———ÿšššÿžžžÿ¡¡¡ÿ¤¤¤ÿ§§§ÿ«««ÿ®®®ÿ±±±ÿ´´´ÿ¸¸¸ÿ»»»ÿ¾¾¾ÿËËËÿ½½½ÿ³³³ÿ¼¼¼ÿÆÆÆÿÏÏÏÿÙÙÙÿÙÙÙÿ333ôÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333<<<ÿÙÙÙÿåååÿ¯¯¯ÿ¨¨¨ÿ¯¯¯ÿ···ÿ¥¥¥ÿ===ÿšššÿÍÍÍÿåååÿáááÿÇÇÇÿ¨¨¨ÿaaaÿdddÿ²²²ÿ²²²ÿ²²²ÿ²²²ÿ±±±ÿ±±±ÿ¨¨¨ÿ‚‚‚ÿ„„„ÿƒƒƒÿƒƒƒÿ„„„ÿ„„„ÿ„„„ÿ„„„ÿ„„„ÿ………ÿ„„„ÿ„„„ÿ………ÿ………ÿ„„„ÿ………ÿ………ÿ………ÿ………ÿ………ÿ„„„ÿ“““ÿ©©©ÿ³³³ÿ¼¼¼ÿÆÆÆÿÏÏÏÿÙÙÙÿâââÿ>>>ÿ333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ã¾¾¾ÿäääÿ§§§ÿ¨¨¨ÿ¯¯¯ÿ®®®ÿEEEÿrrrÿ±±±ÿÑÑÑÿïïïÿéééÿÊÊÊÿ¦¦¦ÿ<<<ÿ£££ÿ¶¶¶ÿ¶¶¶ÿ¶¶¶ÿµµµÿµµµÿµµµÿ–––ÿ™™™ÿ½½½ÿ¿¿¿ÿÁÁÁÿÃÃÃÿÅÅÅÿÇÇÇÿÈÈÈÿÊÊÊÿÌÌÌÿÎÎÎÿÐÐÐÿÒÒÒÿÔÔÔÿÖÖÖÿØØØÿÚÚÚÿÝÝÝÿßßßÿáááÿÇÇÇÿŠŠŠÿ©©©ÿ³³³ÿ¼¼¼ÿÆÆÆÿÏÏÏÿÙÙÙÿâââÿUUUÿ3331ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ222¬›››ÿßßßÿ¡¡¡ÿ¨¨¨ÿ®®®ÿWWWÿNNNÿŠŠŠÿ§§§ÿÁÁÁÿÓÓÓÿÑÑÑÿ¼¼¼ÿrrrÿaaaÿºººÿºººÿ¹¹¹ÿ¹¹¹ÿ¹¹¹ÿ¹¹¹ÿ´´´ÿ‚‚‚ÿ²²²ÿ½½½ÿ¿¿¿ÿÁÁÁÿÃÃÃÿÅÅÅÿÇÇÇÿÈÈÈÿÊÊÊÿÌÌÌÿÎÎÎÿÐÐÐÿÒÒÒÿÔÔÔÿÖÖÖÿØØØÿÚÚÚÿÝÝÝÿßßßÿáááÿÓÓÓÿ………ÿ©©©ÿ³³³ÿ¼¼¼ÿÆÆÆÿÏÏÏÿÙÙÙÿâââÿjjjÿ333Pÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ222uvvvÿÊÊÊÿ¡¡¡ÿ¨¨¨ÿlllÿ777ÿ^^^ÿ{{{ÿ”””ÿ¨¨¨ÿ´´´ÿ²²²ÿ¢¢¢ÿ===ÿ¤¤¤ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿ½½½ÿžžžÿ’’’ÿ»»»ÿ½½½ÿ¿¿¿ÿÁÁÁÿÃÃÃÿÅÅÅÿÇÇÇÿÈÈÈÿÊÊÊÿÌÌÌÿÎÎÎÿÐÐÐÿÒÒÒÿÔÔÔÿÖÖÖÿØØØÿÚÚÚÿÝÝÝÿßßßÿáááÿàààÿÿ§§§ÿ³³³ÿ¼¼¼ÿÆÆÆÿÏÏÏÿÙÙÙÿâââÿ€€€ÿ333nÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ111>SSSÿ²²²ÿ¡¡¡ÿ€€€ÿ///ÿ333ÿNNNÿfffÿ|||ÿ‹‹‹ÿ”””ÿ“““ÿhhhÿ\\\ÿÁÁÁÿÁÁÁÿÁÁÁÿÁÁÁÿÁÁÁÿÁÁÁÿ¿¿¿ÿ†††ÿ«««ÿ»»»ÿ½½½ÿ¿¿¿ÿÁÁÁÿÃÃÃÿÅÅÅÿÇÇÇÿÈÈÈÿÊÊÊÿÌÌÌÿÎÎÎÿÐÐÐÿÒÒÒÿÔÔÔÿÖÖÖÿØØØÿÚÚÚÿÝÝÝÿßßßÿáááÿãããÿ‡‡‡ÿ£££ÿ³³³ÿ¼¼¼ÿÆÆÆÿÏÏÏÿÙÙÙÿâââÿ———ÿ333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ... 555ü˜˜˜ÿÿ888ó¦!!!ÿ999ÿOOOÿ```ÿmmmÿtttÿrrrÿ;;;ÿ¤¤¤ÿÅÅÅÿÅÅÅÿÅÅÅÿÅÅÅÿÅÅÅÿÄÄÄÿªªªÿŒŒŒÿ¹¹¹ÿ»»»ÿ½½½ÿ¿¿¿ÿÁÁÁÿÃÃÃÿÅÅÅÿÇÇÇÿÈÈÈÿÊÊÊÿÌÌÌÿÎÎÎÿÐÐÐÿÒÒÒÿÔÔÔÿÖÖÖÿØØØÿÚÚÚÿÝÝÝÿßßßÿáááÿãããÿÿ«««ÿºººÿ½½½ÿÆÆÆÿÏÏÏÿÙÙÙÿâââÿ­­­ÿ333«ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ÐxxxÿAAAþ333P Ø!!!ÿ444ÿDDDÿNNNÿTTTÿHHHÿXXXÿÉÉÉÿÉÉÉÿÉÉÉÿÉÉÉÿÈÈÈÿÈÈÈÿÈÈÈÿÿ¤¤¤ÿ¹¹¹ÿ»»»ÿ½½½ÿ¿¿¿ÿÁÁÁÿÃÃÃÿÅÅÅÿÇÇÇÿÈÈÈÿÊÊÊÿÌÌÌÿÎÎÎÿÐÐÐÿÒÒÒÿÔÔÔÿÖÖÖÿØØØÿÚÚÚÿÝÝÝÿßßßÿáááÿàààÿ‚‚‚ÿ´´´ÿÂÂÂÿËËËÿÔÔÔÿÚÚÚÿÞÞÞÿÞÞÞÿÁÁÁÿ333Éÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333’888ÿ333ÿÿÿÿÿÿ ›ÿ&&&ÿ///ÿ333ÿ333ÿ¤¤¤ÿÍÍÍÿÍÍÍÿÌÌÌÿÌÌÌÿÌÌÌÿÌÌÌÿ¸¸¸ÿ„„„ÿ­­­ÿ°°°ÿ²²²ÿ³³³ÿµµµÿ···ÿ¹¹¹ÿ»»»ÿ¼¼¼ÿ½½½ÿ¾¾¾ÿÀÀÀÿÂÂÂÿÄÄÄÿÆÆÆÿÇÇÇÿÉÉÉÿÊÊÊÿÍÍÍÿÎÎÎÿÐÐÐÿÌÌÌÿ¨¨¨ÿŠŠŠÿ¸¸¸ÿÂÂÂÿËËËÿÕÕÕÿßßßÿèèèÿòòòÿØØØÿ333Öÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ@@@222.ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿH£ØþSSSÿÑÑÑÿÐÐÐÿÐÐÐÿÐÐÐÿÐÐÐÿÐÐÐÿÑÑÑÿ®®®ÿ………ÿ………ÿ†††ÿ†††ÿ†††ÿ†††ÿ‡‡‡ÿ‡‡‡ÿ†††ÿ‡‡‡ÿ‡‡‡ÿ‡‡‡ÿ‡‡‡ÿ‡‡‡ÿ‡‡‡ÿˆˆˆÿˆˆˆÿˆˆˆÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿ‰‰‰ÿÿ©©©ÿ¸¸¸ÿÂÂÂÿËËËÿÕÕÕÿßßßÿèèèÿòòòÿÚÚÚÿ333Ôÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ222Ì¢¢¢ÿÔÔÔÿÔÔÔÿÔÔÔÿÔÔÔÿÔÔÔÿÈÈÈÿ¬¬¬ÿ¥¥¥ÿ©©©ÿ¬¬¬ÿ¯¯¯ÿ²²²ÿµµµÿ¸¸¸ÿ¼¼¼ÿ¿¿¿ÿÂÂÂÿÅÅÅÿÈÈÈÿÌÌÌÿÏÏÏÿÒÒÒÿÕÕÕÿØØØÿÛÛÛÿßßßÿâââÿåååÿèèèÿëëëÿîîîÿÂÂÂÿ¯¯¯ÿ¸¸¸ÿÂÂÂÿËËËÿÕÕÕÿßßßÿèèèÿòòòÿÑÑÑÿ333Ñÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ENNNÿ×××ÿØØØÿØØØÿØØØÿÎÎÎÿ®®®ÿŸŸŸÿ¢¢¢ÿ¥¥¥ÿ©©©ÿ¬¬¬ÿ¯¯¯ÿ²²²ÿµµµÿ¸¸¸ÿ¼¼¼ÿ¿¿¿ÿÂÂÂÿÅÅÅÿÈÈÈÿÌÌÌÿÏÏÏÿÒÒÒÿÕÕÕÿØØØÿÛÛÛÿßßßÿâââÿåååÿèèèÿëëëÿîîîÿòòòÿÊÊÊÿ¸¸¸ÿÂÂÂÿËËËÿÕÕÕÿßßßÿèèèÿòòòÿ«««ÿ333²ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333ÀŸŸŸÿÜÜÜÿÜÜÜÿÔÔÔÿ±±±ÿ™™™ÿœœœÿŸŸŸÿ¢¢¢ÿ¥¥¥ÿ©©©ÿ¬¬¬ÿ¯¯¯ÿ²²²ÿµµµÿ¸¸¸ÿ¼¼¼ÿ¿¿¿ÿÂÂÂÿÅÅÅÿÈÈÈÿÌÌÌÿÏÏÏÿÒÒÒÿÕÕÕÿØØØÿÛÛÛÿßßßÿâââÿåååÿèèèÿëëëÿîîîÿòòòÿõõõÿÑÑÑÿÂÂÂÿËËËÿÕÕÕÿßßßÿèèèÿòòòÿÿ333^ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333;JJJÿÞÞÞÿÛÛÛÿµµµÿ”””ÿ–––ÿ™™™ÿœœœÿŸŸŸÿ¢¢¢ÿ¥¥¥ÿ©©©ÿ¬¬¬ÿ¯¯¯ÿ²²²ÿµµµÿ¸¸¸ÿ¼¼¼ÿ¿¿¿ÿÂÂÂÿÅÅÅÿÈÈÈÿÌÌÌÿÏÏÏÿÒÒÒÿÕÕÕÿØØØÿÛÛÛÿßßßÿâââÿåååÿèèèÿëëëÿîîîÿòòòÿõõõÿøøøÿØØØÿËËËÿÕÕÕÿßßßÿèèèÿÅÅÅÿ666ó333ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333¶˜˜˜ÿ–––ÿŒŒŒÿÿ’’’ÿ–––ÿ™™™ÿœœœÿŸŸŸÿ¢¢¢ÿ¥¥¥ÿ©©©ÿ¬¬¬ÿ¯¯¯ÿ²²²ÿµµµÿ¸¸¸ÿ¼¼¼ÿ¿¿¿ÿÂÂÂÿÅÅÅÿÈÈÈÿÌÌÌÿÏÏÏÿÒÒÒÿÕÕÕÿØØØÿÛÛÛÿßßßÿâââÿåååÿèèèÿëëëÿîîîÿòòòÿõõõÿøøøÿûûûÿÄÄÄÿÕÕÕÿÜÜÜÿ²²²ÿHHHû333Lÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ333íWWWÿVVVÿWWWÿXXXÿZZZÿ[[[ÿ\\\ÿ^^^ÿ___ÿ```ÿaaaÿcccÿdddÿeeeÿgggÿhhhÿiiiÿkkkÿlllÿmmmÿnnnÿpppÿqqqÿrrrÿtttÿuuuÿvvvÿwwwÿyyyÿzzzÿ{{{ÿ}}}ÿ~~~ÿÿÿ‚‚‚ÿƒƒƒÿ„„„ÿ†††ÿjjjÿ???ÿ333å333Sÿÿÿÿÿÿøÿüþ?þÿÿ€ÿ€> €€€€€ÀÀÀÀÀ|à|à>àÿÿààÿÿÿÿÿðÿþþÿðÎ8xðÎ9Î1ÇðÏ1Æ3ÇðÇ1æ3ÇøÇæ3ÇøÇç1Çøãç1çøãç8Çøà<üw>ÿ'üÿÿÿçüÿÿÿãüÿÿãþ?ÿÿãþ?ÿÿÿ÷þÿÿÿÿþÿÿÿÿþðÿ`ÿ ÿÿÿÿ€ÿ€ÿ€ÿ€ÿÀÿÀÿÀÿÆÿÇÿÿÀÿÿðÿÿðÿÿàÿÿàÿÿÀÿÿÀ‰PNG  IHDR\r¨f IDATxœì½yœÕ½6þT÷éž}ƒ˜`Ø7KY"› "nJŒ1š\£I®Iü%zo¼Yoró¾¹É/ޘě˜¨q•€â¦€P¬ Ã2Ã2³ï½ÕûGOµU§NUŸê®žî~>ŸúÌôéêS§«ë<ç{¾« Ë2’0†(Šwx-ÞãH" txÀÓ’$UXù $cˆ¢èpÀÔx%‰$8 xÀ£’$µñ| I&EÑ   §Ó‰™3gÂëõBýUÿ/‚a»ºÍ®sÌÎÕ¸ÌþÆúš—Âwà=NŸ>ææfTWWãìÙ³ðûýƒJ7K’TîÄ$„(ŠGLQ^ãß@ @uu5ÒÒÒ4?¼ò¿•¶H?þc=ÖÁv?â~¿ÿþû(,,„Óé„Óé„ßïÇÙ³gQ]]ŠŠ ìڵˈÚÜ#IÒ?Yo*H@ˆ¢ø|[y]\\Œgžy~¿ÿûßÑÑÑ·Û þL’âÿììëÝwßEQQœN'Gè¯úÿúúz¼ôÒKرcs9àk’$=K¿¡ÀaôF!ü @ŸòâÂ… رcœN'Ö¬YƒÛn» ²,#ÄqˆI\jظq#†ö¼ââb<þøãøío‹«®ºŠ~ÛàQ}>Ia IÒQ·­[·@²,#;;÷Üs®¿þzäääÄgI\RØ¿? -}fìرøÉO~‚/~ñ‹ô[N/‰¢8”õ¹$ðá|Ê‹Ó§OcïÞ½e9täççcÅŠ¸þúë-ÿxI$¡àôéÓQI“k׮ŗ¿üeº¹Ô"¦ I$©À+ê¶uëÖi@9rrr°páB,\¸0)$a UUUhllŒºŸ{ï½>ø ݼJŇéÆ$ðã?´³Ž?ŽÃ‡“† †¥K—BE¤¦¦ÆkÌI œ8q–ɯ@Ù–Rø‘(Šéê†$pB’¤£þ®nS¤€M0räH,_¾S¦LËåðq'‘ø8~ü8.^¼h{¿>ø`ÈBÕ¡îS7$ À~¦~Q^^ŽÊÊJSe‡ãÆÃÒ¥K1fÌ8ÉÛžDÇŽ‹Éä€áÇãóŸÿ<Ýü-QC^FÉ'Ñ$IÚà]uÛúõë5+¾ Ȳ B¦NŠE‹¡¨¨(_!‰,Ë8zôhÌ&¿‚µk×"77WÝ4ÀÊ‹$XÇOÕ/öíÛ‡Ó§OkH@¤§§cæÌ™˜;w.òòòtðIIJ,ãÈ‘#hjjŠùµ233±råJºùÊ?I°I’vØ¡n{ã7 ufANN®¾újÌš5 ñø:I 0dYƧŸ~: “_Á‚ è¦%¢(æIˆ]ÀîÝ»qáÂ…ˆH@ñ!˜;w.&MšD+m’¸„pðàÁü0cÆ dgg«›œæIˆ’$½`¿òZ–e¼ù曆\ ³sŠ‹‹1wîܤ¢ð„ßïÇÁƒÑÒÒ2à×v:˜7oݼH@4ÐèvìØÆÆÆ°«=`.8”––bÎœ9IEá%ŸÏ‡ ¹¹9nc˜3gÝ4H@4ø;€Pö¿ß·ß~;,¨%³s\.ƇٳgcÈ!ñø~I؟χ}ûöÅeåWcÔ¨QtÓQ]Iˆ’$Éz†°uëV´¶¶r‘¯~ -- “&M´iÓ™™¯šD„ðûýسgZ[[ã=Œ9’nJ0-IÑáeÕÊ ¯×‹7r¯~@–edeeaêÔ©˜0aBRQ8àóù°{÷n´µqeæŠ9òòòX–¦Iˆ’$ùü—ºíƒ>@gg§%à• 773fÌ@ii)œNç€ç$ÂÃçóaçÎ 3ùŒ1‚nÊO@ôxÀåEoo/6oÞðn ??Ó§OGQQQ(›LñG__>úè#´··Ç{(: ªK $€h ‚{ß¾}…]]]ëÕí›7oFOOOT$  œÅ °°S¦Laý¸I 0z{{ññÇ£££#ÞCa‚±Px d0B`¡ ·8ŽE‚ ”¸\®|A„3gÎ`âĉ!‘¼«« ~ø!V¬Xa˵iR AAII òóóQ[[‹ÎÎNø|>´µµÁçóiòÍQßÉ´Ýê9fÿóô5׈¶/Öÿ)))ÈÎÎÆÙ³gáóù@HbN+ä'æH‚ d¸ÁápÜìr¹V9Ž<%)#б­­Mc®{÷Ýw±téRÛB€Ã‘¸Ýn”––¢µµ[·nµåºIX‡ËåBzz:òóó*ù,wP€(ŠÅ’$ÕÅ¢ArAø!äN§3ÎÊ*‚Žz{{!ËrˆeÛÚÚ°cÇ,^¼Ø¶qñ€„A/x½^´µµ¡½½yyy(..Ž÷0%€¾ACý1ÌOX `€nQà—’$õØq ARAxÔår}ßétæ9NB4“ßét2 @!´´´P›6m¢E‹bâÖkF)))¶_/ ëeÍÍÍèééÁ¸qãâîÞ͸~Û !Pç4KðC÷ˆ¢8O’¤ˆý,Ap¸òCBÈB”ɯdPçd7ÚГ²©© »víÂ5×\éЂEC‡Evv¶N=dÈÐ8é½,«îŸem ÷ÉJ?FýõnÏ®î—ç|õç¬|_̾·,ËèêêBkk+¼^/óó===8uê&Nœhxã{ Eq(¯;&xEÅ’$…­™DC„<‡Ãñ!d™Ëå!$t(Ÿ–Œ€îÁÚ´iæÎSS‹ÆŽ‹ƒjÚ†Ê? ó¥ö:VÃ]·££---8{ö,Nœ8¡ù]º»»ÑÒÒ‚‚‚Ä Œg°}°˜WiÓZÖå0&C‚0Ñét~âv»—¥¤¤Àív#%%Es¤¦¦†þò´Ò¯®®ûöí‹Ø$©Ù°¨¨H·¨«‹‰Ê$‰~dee¡´´ .ÄÊ•+™ÏB<ÁÚ Є2]sÍ56l}ÎwEQ¼‹·CA®w¹\Ÿ¤¤¤Œ§'==ùé6³#--M÷ÃoÚ´ @x»È@‘BJKK5ãèèè@WWï-J" êBp{{{ãpúôiôõõÑo ¸Zýb„ ¸þúë‘žžNŸ÷'Q¯×™ ßt¹\ÿt»Ý9n·Êa&( ÐA;gÏž5M!+5j”Î]8Þ«Ð儲²2Ý3Z__óëz½^ôôôàСC8|ø0.^¼ˆÜÜ\VPRm€(Š£„Jí¤¤¤`êÔ©ÈÈÈÀõ×_O?àéþaT A¸ßívÿÚív;y'¿Ñk#ÈÈÈÐýð›6mÒLÎ ÅAH¦¦&x<žè~”$¸àp80}útM[GG·97A€ßïG__º»»ÑÞÞŽ¶¶6tuu¡¯¯¥¥¥5jT(x¬¡¡î¢&á ”ø?vìX! °° .¤Ï àUQu‘2‚ \ãr¹~ïr¹`6ùi"àÝÿ«Ú5·ªªŠ+…¸Ý$0zôhò') [ÕˆKɲ ŸÏ‡¾¾>tvv¢­­ èéé×ë5íW–eVⳃŽ{joo/`êÔ©˜6mý™¥ ¢ôAIyÃÜn7h"P‚™N€‡²³³‘••¥Ô;ï¼cª¸‹Å‘––¦Ó<766Õ”OÂf°Ü‚yî½bªôù|ºÕ½··>ŸÏ’$ÑÔԟϧnj”$©gPÍÕ Ïþc¢(Þ‚ ¤9θ\®á.— ÊÁ"E0"+$@{€;vL“BœÞ³Çê=z´f~¿Ÿ%&°€ž¸Š¹Y–eôõõ¡½½---hooGWW<OÔÛFÙ±³@‚§E‘˜­n?~¼N¤u:¸ñÆYsžEq– ¿w¹\³Õ“ŸEôÄWÿOïýy¶C‡ÕUKE999tVXÔÕÕÙ¶MÂ,P|H<ÚÛÛÑÔÔ„ææf´··‡\Êíkÿ$8˜† b@0«‰pC›Ú222°jÕ*Z)˜6¦¥¥}‘vⳈ Z}½ú–——ãüùó†$ †$@›=O\“T^. ÄnÁ8‘ÖÖÖPÈø@`°SüÀ2¢¨¨K—.Õ´9ŽÂÒÒR¸Ý$ÀšüVˆ %%………)@–eî„!ÊùvºÈ´ . ‰Ø¢³³S×"±Œ-Àà$Åõ’iL›6 3gÎÔ´)¡™ÊD7“h}€RÀ„ 4ã9xð W q;IЧ„êêêJÈÌ5—hˆW®€KB(++ ýŸŸŸoø¡%K–èö¬¬,dddh&?ï¶ ) 55¥¥¥ xÿý÷-M^;H ¨¨H÷ÖÖÖFþë$thv¼JÄ:E1À$åµ 0+šáp8póÍ7ëÌp™™™\[pD`䘖–ÆÜ(ÎKjìÝ»---–H@A¤àt:u÷­¥¥…å"š„M  )ðã*¨ÆWRR‚ÔÔÔ 33Ó”M322pÛm·é\aÕ &šÂYxL‚êsƧ!$¿ß?ü0bq>R())ÑYQ’R@ì@oâ!(¦E<ê€Ä&ÿ¿Z¨èh¥¢¢"f^¾@ `JŠ.Àˆ¬ˆÿêó¯¸B¦ðÉ'Ÿ„\CŠÜn·Î1¨¡¡©­N"z$0Vÿsý…mš - ÔÙwŒ0cÆ ˆ¢¨i“å Ã‹xtB__߀’@^^^Ò1(Æ “¯( eƒN€ý?Ò-€‚¢¢"¬^½ZÓæõzQ]]Êù§&–~€ÞðN|õ1kÖ,Mº®îîn|òÉ'¶-rÆ„@›»»»¢’í¥‚D1‰K €ÚþÉ@+®¸B—¥·³³SGêÃH7`&Т¿2ùN'ÒÒÒ0cÆ Í¶o߯×k ðHº‡2)؇DqT[€þÀ+Õm, €bŒD¤ºþúë1vìXM[mm-.\¸Êø«® >X[3ÅŸš4Ôd2{ölÍäëèèˆiòP@O‚ `øðášû ©$=Aeyp¨ÀÜÜÜP>{‡#")Àápà®»îBnn®¦½¢¢mmmºüÿfD`ä!¨^õYõÒÓÓuÞÛ·o‡ßï) (PÚ† ¦»·I)À$B@[[]¯ E’¤ÐÀ‘ ýÿéœì@dÛ ¨¼÷Þ{5bY ÀîÝ»Ñ×תþC“MÊ$gM~õÄgÕ˜={¶F¹ÙÒÒ‚C‡ÅŒX!D—¾¬±±1™70Jøý~$•h&@`€5XUªQ\\ŒÛo¿]ÓÖÛÛ‹;v„Ddú`I„ÐV %%E7ñ ‰dddè"·oß>  &z’y£D¢ø˜‰ÿÀ !ÖÊX7²0sæL,Z´HÓÖØ؈]»vq÷¡Œb8V#̘1Cs~cc#Ž= `à‡¦¦¦ê§êêê’y£Ë€6eÂ@B•cEŽ3ê×4¢‘¬\¹µµµ8qâD¨íرc(,,ÄìÙ³M'P @ %iôûýðûý\033cÆŒAUUUèÚÛ¶mÃäÉ“ðWŽ………š‡ÖëõâСC¦¶ku»"1©ßSÚÔÒý¿YŸt«¦뺬1¨_]Ëì:Fc4ºí¬Hp[€„""¨@z‚Ó+¬àp8°víZüú׿֤ÉÚ¾}; 1fÌÍd¡‰îóùBIGˆ ”C!å`‘À”)SP]]z8kkkqüøqŒ?^3ÎX’AVVRSSCÙ–èâ$"G"šÄÛèjÆ«‚[ééé¸ÿþûCE€à^øí·ßF{{{h"+“ÞçóÁëõÂëõÂãñ ··½½½èëëC__¼^/“X„••¥sÊaé€Øn X9ì“° ä4x œÆýÛ…ââbÜu—¶¼`WWÞxã x<Í„VÀãñ„&=M‡IjPþ§•gÏž5L!Ćòòò S­%9A%³h * $5hPkìŠd›5kΟ?>ø ÔV[[‹M›6áæ›oiö•ýž"ò+{eu›ZJ I€– 233‘ŸŸ¯ ÈÙ±cFvÌvn œNgè^Z-›­ü5:ÞoÖw¸ó¬^Û¨_Þë±®­~‚‘#G"++ §OŸÖl±b ǃ¶¶6u“€&ûK€Q kågéì¬z{ÓM7áüùó8vìX¨íСC(**Âܹs5U ‘‚z« H x½^Íû Œ5JCUUU¸pá‚iú3v@kk+ '¥ò:’#šÏ&Âç£í#`ôèѨ¯¯§'eÌpñâEú¹8/I’Æ´“H[ÿii©fRé€ÈŒ î¿ÿ~]âÑÍ›7£ªª '$ú+b?}(Ûe+À"µ4àóù••¥Á?úè#ns^4â¿ÇãÁÉ“'m½Ih0|øp†?Ù„ÿÄ"K@ê6»j¤§§ã«_ýªN)¸~ýz\¼xQ3ÁY“ŸÖ¨ @½% ·ôÃQYY‰†††˜’@ ù$[ȲŒììl®­]´`¥–ÆŽËœð@lL,ã¾ûîÓ´uuuaݺuèîî6\ýi @MFD ééé:‰fçÎÜ“Y ÞÏœ³ª[G…ÁD¦öÿpz€´´4Ãm‚]HOOÇ׿þu S¼÷Þ{hjj2œø,)€v¢¥%Q©ú;ùý~[Ò†A8zô¨Ž’ˆ†ªËQ -YXPˆ "ââb<øàƒš±ôööâƒ>@WWWD A³-½7<|øp(Ì4¨©©P‡”$ø ËÁà0ºpK$hoo§s:´K’¤SöÄDQÌ0Yy-FÍ-þ+í±²И={6V­Z¥iknnÆîÝ»¹tf$@“+ ÀëõF6¬¥¥…µ2$‘@p»ÝQû ð¬þ@¨€ÅÅÅ3ˆ)…[o½U—Ú»¦¦•••ÜR€úPb XRmž;xð z{{#šü^¯7éì3Hàp8PXXq1à a @W€†Ùʯ –¦@Öu¿öµ¯é\t+**PWWgY 0óèííÕÄ9ôõõáÀ‰þGŽ°{”„=6lXD‘„ƒIÐYx€Ôm)A‰ã[ßú–溲,£¼¼mmmÌÉoÅ* >è8‡XJ!ÇO„eC‡Õx¤ò`0€¡À³ò+ˆµ)…ââb<üðÚñx½^;v ÝÝÝ\“ŸÇ4ØÚÚª‘zzzpøðaîÉ_WWG—‡NbA–eäääX’rÅ@Å‘Br´º <Ò€Ý6T^Ìž=[—X´§§gÎœ1Ô˜éÔ“_ù¿··W“©öïßJCfvôôôàܹsyK’ˆdYFjj*÷B7X$Íê?jÔ¨P¬=pÒ@<$·Ýv›®y{{;ÃZx¥€úúúJ¡<Ó¨¨¨KÉ ŸK .—K—À•ž@ Á@ídU ¤ãŽAÀ׿þu]ÅÝ––tttpY˜ëz{{WxVÝ¿™I@ÒÓï…Ãá0ÍÜä÷ûÑÒÒ¢n `Š EFXívç°‚´´4<þøãº}Z{{»fÕ'0È`Ÿßï¿J–å“~ äÝÑÖÖ†ÊÊJÐMþ¤³Ï¥¬¬,¦™‘¤V’$¦8nÀŠ3fŒe  æEQQ¾ùÍojƤìä£ä!ýÇk~¿¡,Ëç@’¤sžW_S‘ÔØ»w/KLâƒòlÑ5x€@|%M @:0"H_#\y啺Ģ²¬À’hË@ÿ!{<ž§ü~ÿ²,Ó•: ”Ö©¹¹§N ­þJ.ÿ$. 2iiišzŽƒ…Šÿ€y*0‰@°fÍÌ;WÓ¦ˆj¬}?Eç¼^ïm@àG¬¾%I: à5u›"ÔÔÔ$mý—!²³³1bĈPq^ `‰ñ´¨!¾ùÍoêÒ>¥¥¥A–e£¨ÁÇó¸ßï/Ëò?Â\âgBrcc#Ž;†ÚÚÚ˜GE&‘˜p»Ý˜4i***,IñÌ l‹úÜD! ¨|â‰'ðío[ãÉ7dÈtvv¢¯¯O©ÐãóùþY–.Ër+Oß’$•‹¢ø6€ÕJÛ¶mÛ4¢¿å©)‹EÄVûR¿¶Úo_¼×`ýoµO³>ìúŒ•¶šÝ|¯¢Ä…Œ"iXñèèèÀ¡C‡PWW‡üüü¸Zá»ßý.~øÆuJÇñãÇy½Þ7üN–åótÿ«Ð/Å)…F’H‚Â)›ÞŒ×@S°°°©©©9)X¿~=$IBGGÇ€`0ÕW^‰/~ñ‹š¶ÔÔT̘1ã,ËOF8ù!IÒ^÷"Xì!‰$Œð## ?ˆHü7Ò´¶¶¢¶ö³‚'²,ãܹs SÞúóŸÿ<.\H7ß)ŠâãÑô+IÒËnp&š~’¸$!xÀßÌNŠ— * ú=õäWàõzqöìÙr1Þxì±ÇBõþTøOQJ’d(¢…ƒ$IlE1€zß#püÏ{Þ@}&9{Ϋ—$é80`Å§@mU£«« uuu(..Žv¼Q#55O=õ}ôQtvv*ͯˆ¢x•$I§¢é_’$.bI¨1à[:Ðívk&h$@F"(¿è¸¡¨¨O>ù$í¾™àï¢(&Ž #‰ËñÐ0#iðêêëëM/xîÜ9twwG2VÛqå•Wâ ›§øËÀ&‰Ëñ M Ñ£G[Rü)í tÊ>Zä—eÕÕÕ ã%wçwbñâÅtóQ¿ñ$qù"î@´@´øïr¹0cÆ ]¯×‹ªªª„É…ÿøã³ÒŸÿDÅñO—'”Œ"iXÑЀœœ‚€Y³fé²uvv&L)¬””üô§?¥ãº^Eq\œ†•Äe†–¦)»rrr——•Û*‹€‚«¯¾Z—$¤¡¡!aBe ñÿñ´$À?DQŒOŽ³$.+ 4hÄÿÒÒÒÐÿ‘8±rå©K+eff⪫®ÒõY]]­6ÅÅW^y%~øaºy*€Çâ0œ$.3Ä•Œüÿy‰ ®®Nwž"((,,Ä”)S4m²,ãĉº¢ñÂwމ뮻Žn¾"cIâòÂ@;Y²° n§Åÿ´´4¸\.Ýg&OžŒ¶¶6œ?ÿ™Û½ÇãAee%¦OŸníÄ·Ür 6oÖ8NŒ×X’H|ˆ¢Xà«ÊÜ6 †ˆ×8àO’$…0è -Å‚ „Ý„“Œöÿ,Ì™3ï¿ÿ¾&?~GGªªª0~üxËßÇ.tttàܹs¬èÅø‡3&‘E±ÀaCLN»À#áúH @ÈÊá­À„,\¸›7oÖˆþuuuÈÊÊŠ‹»ð‘#G0räH̘1¯¿þ:ýöžÐe Q—ø&€1Ô¦™‚«ê~OJ’”Vþ?˜O~¸ FL`¤€¶¶6ôôhÓ噥J‚YTçÏŸmÛ¶iüNž<‰ÌÌLS±²,ãäÉ“˜4iRÈJQ^^NŸ¶{@“DQ\“˜y|hÇ IDATIáÅN¬xCZÊ@øâX% ¡0 zõw8\JJJpÅZýš,Ë8rä]O=&P*þ¨·?@’âŒ'8ÏÓ¹o4ú}i¦rœÚÇÓ_B€‚hÄÿÌÌLîRÊÓ§O×åëóx<(//YV%…3ËòÐÔÔDŸ>c2$4EQåœf‚Dð%/ ×tëÑÀÓÙ€€(Š#Àˆ´*þ›Y‰ÿ4.\¨ñ‚E<Ž?n©ôõõ¡££Ã0A £l÷~I’b/Ž$%à—$BXéðð§ô6rJЬþ#GŽ ­Ö‘ùý~44h Î*¸\.,[¶LWvùÂ… ¶Ó hooGww·i BRü+JßB"ø‘rž—¸PZZjÙä§ÆÅ‹u«©UP>³dÉ]ÿ•••hm.¿Foo/ššš¸ô I‡pÚt5áwü€²ÿ·ê¤¼G{B". 2räH»°,Ë8xð`D‰Eý~?š››ÑÚÚÊypì˜.{S"Îóêìå@81è$M`vv¶¦ +D@K<æ¿pX²d †ªikkkc‰ç:øý~Ô××£±±Ñ²!)þÇ’$ð5}Xð!èt‹$I‰LÂVÀ@8iüÿ#uRÚ{{{u9þ¢•€ RpåÊ•xíµ×Ð×÷™ õܹsÈÉÉÁرc™ŸëèèÀÅ‹áp8t¡Ç<8zô(Ý”$€†$Ið‡þÌÊjÓPA‰,1¢Æ‚°Uˆ(˜A˜ùÿ쀠+ñŠ+ðæ›ojöïGENN Bm>Ÿõõõèíí!„ÛF’ý™•=»ò ³€‚hÄÿÔÔTf`¤(--Åüùó5m²,cß¾}¡½zkk+Ž?®«Ënt†"‚’HÂ<@€®D0 1%Và¨Q£,ý Ûb±ÿ§qÕUWaâDm4n__öíÛ‡ªª*ÔÔÔØâ1xìØ1ÚRð©$I‰‘¾8‰„ƒ(ŠÀа'’$q•ÅŠµ ‰6lX(ì•·èÝF[bApà 7hD~ è²­€Iñ? ‹}¥ ¸L€@ì ÀV öövmÞ .— ×^{­®ÝÎzƒtS’’0ƒ­û öÀmà!Zü±+¨ÓÔÔ¤y©¦ŸY–“@IX…­ öV[-4dddD¬}ç]s€ŽˆçΣ•ˆÍNØv‹˜;wn ‚eÊvïÞ›HæÌ™S `‚QnùýÇ*².ʲÜ@Ú³gÏÉ×@BÅT©õm—bFý€¡4;t @ñv2C, €!þbÅÑdáÂ…EB0YE‚{C?€›wìرÉìó ,`iñÕ&BåRß‚ ª$£¶}ôÑGoóŽ‹óçÏŸ×Ý„îä§óçÏ?.ËòF¯ïܹs§cš7oÞPŸWgS€3»ví²• çÎ;Áû>N9!%„¹sçžðiÿ±c÷îÝšûît:• YýGŒ‡ÃQò ¸÷nlÔZ6èÂv¢««K“?°—¢ÿ/^|!äc¨<,)|sñâÅ+·lÙòã³Cü’ò%°J)&÷ðík¯½v7€;·nÝ•‹òç>÷¹y~J±œX£ßZ2¡ÿøÖ¢E‹^ðííÛ·G¡·hѢŲ,ÿÂJ Ù»páŸíرãÇÑ\,X ø!äVo½Kú|wÁ‚;üëG}´\.W!ž“Ä#:‡Uñ¿±±Q§€‹%ÐÖ æ7Bžƒñä‚["€åË— ¾Dù%(SÇ5ÀûË–-›ÿþûïsÙ—ÕXºti€?Bn²úYª±Þ`å’%KûðÃÿI_K–,É&„¼ ÕÞšº©~´xñâW·lÙQ¢ˆk¯½v"€§ !Ë"øø5v]{íµnݺõÏ„`ò»q[Œ¢u¢= !̤¢vÿ !†e½½½8}ú´ºIgЕ+WNp¹\³€°7T1eÅŠù„õ™} Lã<à»<ãTpà 7Ìp¹\dYiåsáÐ?Ö ¿¿îºëü›7oþ“Õ>!«` X£îÅt– `ùòå ].×? ‰¢&¥Àÿ.[¶,“ÂÔ0úŽ¯ÀŠT€¬VÿQ@@,W@/ØémxâÄ Ú‘¨’C „¬à|˜ŽÀM7Ý4„ò>,1éÿ¡U«V=±aÃ.ßøU«V­ „¼ *ãŽZeYüqåÊ•7n|ÍÊg !"Gÿg~=5V¬Xq'!ä¯n©èþ¬Bða“Kô÷÷-ÀT¨DÔ¬¬,äåå…ÞŒ$ˆVÆ’dYÖ]ÏNˆfÿO ™VÃïöÛoOð/¾‡ ™Ð Ù¾à;aÎ 2‚ç=å^|`‚äW… 9v‚º—Á7ñ•þ¸÷ÿÀ€ÚÈ ( óÿ«Á6 ˜rŒÜ}÷Ý9„ÓÒáýÔ?Cð‚AF?ðÓ—_~YÍlŸxç _øB€ßòŒA–å°± „ÿEp××S¯¾úêŒÞ_¿~}€_Ýyçÿ° Á‰a†¯ßyç¿~õÕW¹&,'ø\{€;î¸ÃMy ozª7<ùú믳’Nl€5kÖÌ°*“zp‹ÿÀÀÈ‘ZýU3 -Ž§¤¤Ø:!iÐ`çµè|à©ûÇq%øV–a²,+÷#Ö¾ð Lú} œãÄ€{ï½÷&BÈŽ~~üâ‹/N~5^}õÕÊ{î¹çA'‚R|À/Ãõy÷Ýw§B ԽôÒK\þß„˜Íqª à‡~ôòË/›îÖ­[·ë®»îºÁtdLéB|  ¿®½.†wàx+c¼úK¼‘[á€p«öRp¯øÎLÀk×®AùWÎ>ÿí…^øç¹xå•Wêî½÷ÞÁGqß0#­Šÿf€Xúÿ÷õõé2ÙIŒºQïÿ p÷ÿøÇ°+!D©.~˜œøÊW¾²„2Çè}¾ÿ¿ÿû¿–²ìüõ¯õ?ðÀLße†<ð€ëOú“iÿ„^³$ïvâ{àÛ«oðŸœ×V÷o» ˆHü7jgEÆR ·€½ ÀH÷ÿ€% þãw¿û݇œýŽáì³á™gž1t%„ð”Øê‚y>CB.rœ–àJiê*l§ðÐC¹\®€°Ú}À#Ï>û¬eÀ %^ ¥½úÇ:&Å}Ùx½^TUUÑÍ\ðo|#Ÿ2šóR‡TúqRþ,À£7}ôÑbBÈŽ>6?ýôÓ–íê@á5o‚}V „< ªœ»¼÷Ì3ÏTs^—¾Æà$»€ÒÒÒl›,Ðû§ÓÐ:fÕÕÕðù4ååNK’ĵgëWòâ…_ýêWÜA,„ÅÌŽkÔÇjôo#¬‚BxKså‡;Áår¸ìña%BÈFïQýÿ!\_FˆE`3Ѐ.— ……Ÿy/F’ l ÷ÿ»èˆ]8qBíË-þ»\®«® àe+ã²°µ0ÌWH¹™ncŒ5àŸü#Ó]Ã6`é î­©ðØc‰„É“ÑW-",,ò¯ÿú¯!DãaòÄUÐ$)))Ñ­ VR\¼¨ÝöÅ’ZZZ4)Á{%€(€šIjòlûÙÏ~vž·ß'žxBp¹\Wrz¦1 àûßÿ~–™ø¯ê»êç?ÿ9WÕZ\.—›êÏ<`¸ ú7%BÈfï«úúË/ùˈª B†À ‚ã^ÄÕ ÀŒŒÔˆU0–ÀJ9n'D#­ÒŒà%+cêƒÍ1éOA7]Ãþ>®‡\–å¨J/[ˆ† û\óèdYëDY®Ÿ~lãþøc¾ÁNˆ…øOC–eËÅ+y‹dY¶ƒb"0Žlq²`=‰xõï¿NLL€€½ K¦†U"ðxàŸš½i—àr¹Ò9ͧQ•“²@– Ö–Õ8Uy-‚Æ ÄRxæÌ|üñÇ:£ÓéÄâÅ‹1qâDȲŒÙ³g#%%Ÿ~jú<…C‹Õÿøƒ>hi‚ÙäÄK¶úík_(@è¡ CEq“4€Ls ½°‹ÊËËqèÐ!]{jj*n¸áiòõM™2.— ‡ŽøšD1Sšõír¹²9WÁ"€°lm£€™•–q?¢%€„×0íÿ‘:µ··ë<ò¢%€@ €={öÐÙxyyyX±b²³³u?ž,Ë(++‹Šˆ,íÿŸþy7!dF´}Ó€ D¥Pq¹\WpMXàŒð#ÌÞ݈¨ú˜þüç? .—«€êωA, +â¿ Q¥ïëëÃîÝ»uf>e¬Ë–-ƒÛí6-ù=bÄB,oºººè#?,LV—Ë%\?¾%  òóį·Ü}÷Ý: ¦ª¦£ c¼¼ŠB&œNçJÎS·›½ùâ‹/¦púÕ×®]»Ö4Q !¤‹ÕÎè/⇗ålG=€PÿŒ‰…°€h€”„"‘ ££»víBW—þ·š84Ö­[WÆÒ°3Æêp{P°D,¦ŠEBˆéC§êŸÇóŽ‰W_}u¬ËåšLõÇB;‚IO Î HÕ?O°ix²ª/%®Ñ5 ÷ÿÔ½h¾çž{¸Ë4ýG2(5DQTÊø,0 š"]ý!I’.ï ˜3g¦N ê-ÍV~C† ÁŒ3X•}˜ˆ¥ùOõ¹õÖ[Ã&ë¤úÖíÿ &—©´Bá î‘eyßȘ×XÅèuê?׬Ycšk€WÎ °¿/ÞtáÑ@Øýÿ½ˆHÇb‡ ÿ‹‹‹5tV‰€ÉþÿÌ™3(//×=(.— ×^{-FŒa8ñÃIÙÙÙ˜:u*+ºO‡Xãá·´ÿ7ë›Ñ8hD0Ì7œÛdæ[o½å^½zµõUŠA ¨±®çèËÎD ¼ŽM¼Z|Ö5b¦b@t^=@SS“nbZ‘dYFEESÙ—‘‘eË–!//ÏT”äÙ¤§§câĉ¬ ?š~"Í„×Òóøé³°iÓ¦tBÈÔpçõ÷oÚ÷êÕ«ý7n¬VK/ËòÕ>â'üóŸÿœÉ“iH–åó홂—8%^¿ ¶W,$‚I°b÷WÚÐâ¿ÓéDJJ ý&ü~?:¤«" ùùùX²d RSS-‰ü¬1*HMMŸqãpæÌ&iÔ××Óº‡púÿÿþû^WS«1ýl<~ÎË—/»ÊB6x”£¿û`Þ}÷]ò;ðõ§×_½Þ«‹‚Í/ܸiÓ¦´+V„Ÿ7ntB–sž>ðÀª¨¶DâDO^ÞðßÞÞ^>|˜é¾[ZZŠk®¹N§34ù­*¤BJKKqá±0VÿO$Iâ²eB&€¯,–Á€Ü°`ÿç’,!oƒ“>üð×,Y66þƒ>pBþ`G¿§üžã<[³¯X±¢þ½÷Þ;@öªE€/x†óÚؼy³“k NçüH\$€)Bî™™™™š€ ¢±ðˆÿ(//‡Ç£ß^NŸ>3fÍÝêIlgJ‡Ãââbœ8qB“A8JñŸ×þxÁ‚–öÕ1 €mÆà‡ó tX¿}ûö»-Zd˜xÛ¶mã!ÿÀjÎqþàsŸûohµ­õ!ðÇ©ÿwË–-/^¼8,Yøá‡Y„߸›g ýˆ 0íÿ‘:õõõ¡½]ëÍŽ.^¼ˆÊÊJÝêëp80gÎŒ=š[ä&Ù¨ (,,Ä™3gPTT j°u’R}Ûê\´hÑ"ÏÎ; à¿8N àÝ;wnG°^€ójÊÇȲ| !ävð‰ýðâüùó_à<——Â:©úû=‚5 ÃùT¤ؾcÇŽŸøíÂ… 5ÖŠ;v8,°ÖårÝ€7Î@A hÅÀœÎ;‡šš]{JJ ,X€üü|K+}´RAff&JKKQSSƒ‚‚œ?¯É!ØÃÛW¬\€÷îݛݿ½à7¹B~ ˜Ë›`QÿÁço± ÀW8¯‡Ý»w»9«jçÎËU­iáÂ…u»vízÀZŽÓs¬Zôã]»vU!X¬5@!!d€\ âç0.fÀ°+zš\.3&_–eœúè#Zò¨”$‰7«­¢¨ãÁ'VÆçr¹fɲÌó¥ÎÍš5‹«(&\}õÕÞýû÷ÿ ‚Ž8¶UR1ùýN¸åÊ+¯ä®1àr¹†s~wKy \.׿¸N–e^s_*‚DiJ–ýU‰y·,eŠ˜X5GŒa©èÝÎò¤áóùPUU…ŽŽÝ{ÇÇܹsár¹"¶ñÓˆ„@š››±iÓ&ºÙ´P…GŽÉã 5­6m¿["¢ª.¦ÿw¬ô ³gÏÞ~øðáÛ¼ªPF´ Æú€û®¸â K>ö„9)Ìw·tOEQ¬9xðà-¶ ÿ{Û cÚ àNg8újž={¶¥Tp ¢‘D¨öiùùùs]$ž€´@‹ÿ½½½¨ªªÒ @YY®¸â ‚µß ᤢ¢¿ýíoióŸ+B4©LÆüoŸª¾s8ï¥ì f̘±áèÑ£+üá•‚aAÕ àßü×Ô©S#)¯¥ó–4¸–3øÎœ9swyyùýÞ·ÐCáóö €û,ëÞ‡à6Á¬?îÅ…F4 ñÿ§÷ÿ x‰€¨–:;;qæÌ] ¿ ˜>}:ÊÊÊ"ù­Hfýoß¾Ï?ÿ¼nŒ~#IÒ1ÞkLœ8ñÜ©S§ªh NP×>Šàd°BÈM‡n“ïòÇñãÇo±Ú·‚)S¦l9qâÄ8Y–Ÿð0Tp„èð¬,Ë¿š8q"—vž…©S§^¨¬¬üÀ|ú=Õ½øÁ hÓ¦M{åرcÇü·,ˆú ðÃÉ“'‡2;WTT¼àúDÕx;ü ‚kˆŽt @+E?è6zõ!DÍÍͨ­­ÕM>BDQÄðáÃ5"ÿ@ˆùôç×­[‡wÞaJÍÏà)™­ æÚÀ|–lUF°:o…,ËÏ=Úr½qãÆí=}úô7"è½çêÿþ^÷‘ÛÆŒ£Û¿XÅøñã/x¬ººúײ,À ^ý@3‚NC[ àÍñãÇkFBÈWÜÌDЂ"¨>wF–åç'L˜q®E!Ò‡_ÅsPýË¿ü FŒ§Ó:‡æµYûž={4é·RRR0zôh4662sö¥¥¥aîܹÈÊâñ•±+Ò×ׇgŸ}ê‚ñdÿ.Iw‘ÎK555:ö” 8ù† ¨ùîCp·ôG”5Ê>g8áôéÓiƘ ËòDãš48`Ϙ1c"ªd"’Œ"iXQÒ€ÛíÆùó癞}yyyE)))1±ñó’bkk+ž~úi–)²À}’$½Î}ÑË£FêB˜pÝK £GîA0IItyåb„H·ñ¿¨¨(TF›w«ÏõûýhjÒFVvvv2'bqq1f̘ÁïÀnå_MM þçþ­­:Et€Õ’$YNÑ•D [ ¤D›cÁj +5aLJñãÇ°ÃÏB4z‚C‡áÏþ3Ëõø€›$IŠXQ•D‰H ¬ÀŠË¡G ‡ÃiÓ¦év²úæE¤Á{ｇ7ß|“õù ¾ I’=Å’Hb`™X5£ub¹+p»Ý˜9s&rssÃNÚXÚøý~?^yåìÚµ‹õöøŽ$Iщ%I$1ÀˆDÐDfdd 777ô¦@ê¾gÎœ‰´´4ÛbøÍ`D ÝÝÝøÓŸþÄ*ïåðˆ$I°<¸$’HDBLû? ^"e™©é:t(¦N BHÄ6~;$‚ÆÆFüñÔ¹)#˜êùI’.+­v¢@Eõƒ`ô¿Ù{—êÿ¼Ig@QZuRÚA@nn.ZZ>‹“1bÆÇtëÈàž'Nà¹çžCw·Îƒ´ ÀV¼ûŒ Šâ÷"IW¬z+¦Dû? ˆ¢øgß–$)l¬„mX³û«Û¯¹æšPô\qq1 ¹Ýzcåõ·gϬ[·ŽåÖû€[%IŠ(øB Qoð*¢w•M" 5¾ `…(Š7J’tÀìDK@Gz `]ŸŸÊÚX[åí–dYÆƱe ÓþyJ’d9³- QçxÉÉŸDlP„ Ti ˜ÿOhV´ƒW7páÂ[ÓuEª'ðz½xùå—Q^®+çn«[¯(Š¼(*Æ\î0ûy¤Põkžç×ê9±ø¬YŸÊë––õ3}óÂ*X%€ˆÄ³Äãñ0«ö°ÎåE$ÒÞÞŽ¿þõ¯tÀf·^Q‡ØªRÎC=„ŽŽŽPHµâY èu&Ê_¥þ‚ÑyêÃè}Öµ¬ôÅj7¿zÌ<ßŨžïe¥-Òscý>ïwinnF}}=}ôQµƒÚ$QÝfkTÀ²X!At«? ±–.\¸€¿þõ¯º|„°Ù­WÅ ÿæ{Ï=÷`Íš5غukȵØî‡:Üû‰ôôXJ•®é‡fRQQ×^{-æn½ýT¯‚r¢ºîºëðÅ/~²,ãsŸû^xádggÛqÉ$.q444 ¡¡S¦LÃá`¥…M鮜St !Ç–Íl³ Ξ=Òö[=@T‡,Ëرcþö·¿±&ÿ löéÁüfÏžo|ãšqÝxã“b—X‹—,Ë8xð œN'Æ:‡áR–¬Hÿÿ¢¢"fÂN#QŸnëîîfæñV$¿ß 6`ß>faÛÝzEQü7“P„PVV†'Ÿ|‡Có#çääàšk®aåH" ø|>ôôô ¬¬L7ÿ6Q¨àÿÚÕ ššš°î½v‰ùjôôôàµ×^Cuu5ý–_–åGöíÛÇUa†¢(ÞàÇê¶aÆᩧž2ÌgP™*Á¥%IDATTT„ÆÆF\¸À”7‰Ë]]]ðx<¡$:4b-ØæÔÑÑÁr°ÑÁnå_ss3^zé%]î¿ß³gÏ:[[[¿*B•,ˆUk¬@Åå¦z !33O=õrss ïÑäÉ“ÑÜÜŒÞÞ^;†’Ä †×ë eÁVWÞ¦3EÑF ñ_Yý#U­°š@jjjðÚk¯éö×>Ÿuuuðz½HII™éóùÞu8¯Ë²|¿,ËÜ>Õ4DQœ‰`¹êP<—Ë…'Ÿ|#FŒ͈ä¦L™‚ýû÷G:„$.´··£§§'”JÏ ôÂ%€© "srr,›ü{e¤+{¤Ÿ;|ø06nܨ“:|>Z[[áp8’’ºÉ‡ã¯×;A„Õ²,[f+QG!Xª:”°P|ë[ߤI“¸"AÀ!CPRR’Ü \†ðxŒ&”ÏçÆ pì˜>`O¥”UŸžü*&ÂA¾*Ëò_ÂEÅÿ/±jÕ*¬Zµ*¢œ‡'NÄáÖ*€'1ÑÛÛ‹ÆÆF°â> …8ÔMà(p•`ÅÈáp ¾ž]¿ÐÎRÝ@P[úÆo ¶VûýAÅ.øý~fºrurÓþà à9Aœ²,ÿÉèšýñéσ*x9wî\¬]»6ªRe999())a¹)'q @–e466¢­­ÍPÃ---ô·Q’$o¸Ï…%€~÷Õ©ê6õ€×pþüùˆyZ‘ðÆoèj:NäææÂápÀï÷Ãçóé&?5ñé®'B¥,Ë\úÿø¼ºaÒ¤Ixä‘G“/ù5 MMMÉ­À%†îînÔÖÖ†$ ²1 À'hj:”Y´3œ€¡¡äï ©ªªÂ† àõj‰/%%Ç­üôêÏýYà–ey½ WÑŠAQ¿ à1u[qq1¾ýíoÃétÚ’ÎLŒ?ž©˜Ä D @}}=š››#^õÕˆÄð€¡øÏkûçqú± ú:ûöíÃöíÛud‘2Y*+¿°‚Rô» “eùMAæË²Ü ¢(ÞàWêssssñ½ï}/T¢<°>—‘‘âââ¤U`£££ç΃Ïç³¼×7B,%Ç žH@€U<#êH/e‚lÙ²Ÿ~ª/¼RTT„‘#G"„V~BˆFü7ý âfçÜ)Šâ|/BS‘ššŠï|ç;2dˆí¤°---I¡Aˆ@ €sçΡ©©É–U_ †À¥0²,ðf‚Š¿ªª*À†hÑ×ׇ7âìYmÌŽ ˜0aB¨Ž€BôêÏ)úk ÿïç Þ+--ýúëÁA=ã>ŠQ£FÙníPc̘1¨¨¨°¥¯$mmm¨®®->€=s š@ ˆ¢X äòGÁ°aø€¼^/3㯬L¶¶6¼ýöÛ´é.— ³gÏÆСC5“ßhõgMüp‘‡‚  ¸¸ø¸Õ×¾ÿþû1mÚ´¨~áššŠ¢¢"ÔÕÕÙÒ_±ƒßïÇ™3gÐÐÐ`ûª¯F¬¶šÕ¿°°P³g1sr8¨¬¬ŒI~¿ÚÚZlܸQ'gdd`þüùÈÊÊ ­Ö~¿_#‰þô8ÌÂŽKJJàr¹4“ÿ–[nÁüùó¤~ (jkkK†'0š››qòäI[÷úFˆÕ@§äuêííµ}Ÿ**++±eËÝD+((Àµ×^‹””¸®¬þ,Ñß¾?ÔGnnn(m—‚… ⦛nðú#GŽd+I"Îðù|8qâDD«~¤Ï€HEEAïÖp&?§Ó‰£GÚ¾Þ³g3†ܸqX¸p¡fo¥^±Y«?-ú­újq»Ýp»5 ?¦OŸŽµk×Ú^¿¯Ün7 “[Bcc#Ž;³½> >ŸNgç`\oOC`EòZ:;;áóù˜ýFr3|>¶nÝŠªª*Ý{W_}5DQÔMfz³Dz\f“_–eWZZŠ‡z_µâh¢›››Ü $<*++Q[[Ó½> MMMô³RÇ›ÐÆLÐÔLOO×ÔT@O&§Ó‰'Npï…ÃMŠžž¼û@B–/_Ž‰'†Vt4íýéóYÄá÷ûáñxt“???<òÜn·åŸhÀº_%%%8vì˜NáÊ’Ò"ùßêçb}­húW¿ŽöZ‡eeehooÇ‘#Gd¯ÏB¤ûÀœ"rjnnæJö¡ÀlR´´´`óæͺ;==·Ür JJJ˜“šÞÿÓ“_}m£½¾rôööꤙŒŒ <òÈ#ÈÌÌ´ÅÖ­¾ ªª §OŸ¶=¦"‰ðØ·orssáv»d¯Ïê#Òý?`”ý¿ÖCër¹˜bºUB0aèÖ­[un½¸ýöÛ‘——Bˆ!Ðâ¿™¹5ñý~?zzztäãr¹ðµ¯} ¡Éo—SS$¨««CeeeT×O"rȲ ÇcI´‘ºHjЪ®®ÎÅß‘#G°wï^Ý ;v,V¯^ŒŒ ¸\®8NCPÄ2–ÂÏhÕ÷ù|¸xñ¢f ‚ à¾ûîØ1cl­dd„p÷­³³3&œˆ·äeû€XTTd*þ»\.ÃT_VöÈ{öìa®h³gÏÆÒ¥K‘’’—Ë:!­>ÃÕ_¹†Ñª¯ÖÜž?^·•¹í¶Û0cÆŒ˜(üX0»o^¯û÷ï×1%%E·5K¸q¶ÏĘh}Ô@*üXˆÅ@SpÈ!Û7‹Îœ9ci?LÞëõbûöíº@‡Ãk¯½³gφÛí†Ëå ™ãÔR‹Ô«?~òBàñxPUU¥ »]¼x1-Z4  ?!X2ýðáú²åYYY(++Ó(ÔÔ¿•ÑëD8w°ŒÉëõâw¿ûæ¾›)þbmb³ë|6‰N§e[´úKuvvbëÖ­hkkÓœãv»qà 7 ¬¬ „Bt –”ñ(“šžü,‘_! eqüøqݾÖ¬Y:GŸHa‡¾àÔ©SºÝív£´´4t­ë0{M÷sÕ¯eü¬1Ñy&€øK1'3 %à'ÒÉqñâElß¾]·âfeeá†n@aaahò«…P&0ðYد2^Öª¯žøŠqüøq8UVV†»îº+$=°`eRG»"466âôéÓš6‡ÃÑ£G3ÄÁ8ñ"ÿ@Œ‰Q?2.¦?5ba䶂ÀJF¨;…3gÎà“O>aºõ.]º4”ÁG=QY$àv»A Mveõ 1*{|uŸÊÿ§N™3g4ã>|8¾ô¥/ÁétšNÜðú‚©ÎXQ€#FŒ%i¹T'^¢Œ‰&€xM~å™ëéé¡·‚=’$éãï  #£@‚ ø9vì˜á0›ååå8zô¨®}Ô¨Q˜7oÒÒÒB× '¬šÔ’€RfK1Òº:X‘jjjtù²³³ñÀVï «¢~8ñù|(//×)ýòóó‘““êãRxÑŒÉÎñÓÛTõB477ÓM–²Å°$Íê?lØ0S÷Yú†˜A‚)¹öîÝ«‹á€ &`Ú´i³}Є "(“5„&¾A²O‚€úúzìÚµK3Ž””|ùË_FNNNÄ?¬ÝDEE…ÎÝ733SS •¾~¼'^¢ŒÉÎñÓ:;%€Hž™hÄ€Mº" j(Çét¢¼¼ÜÒ {{{±sçNk)UpF¶#Rp:HII ¹íˆºŸ––¼÷Þ{šUÕétâž{îÁðáÃc’ÑǪd·J¬¼#FŒ0\åÔHÔÉ”èçò(ã­ŒÆpH4(ðz½:3”ÚÛÛ±k×.Ýg\.&OžŒüü|®~eœr¨5û}}}¡øŘ>tvvâ­·ÞÒ)o½õV”••E•Æ;Üø­ ©© çÎÓ´9Œ9Rm6˜&S¢Œ)’s­èb[`ë€Èr"„àÀÜ^õõõØ»w¯Î§>55eee¡ä™á£H=eÏïõz!Ër(õ—r¨?'Ë2úúú°~ýz›/[¶ 3gÎŒ¹ÂÖ}ëééÁ©S§tíEEEÌÌÌ@ü'H¢ŽÉŽs}>Ÿn‹µ0Üó ÐKS ªg—žžR0Ÿ‰ß]]]†ùééWWW3· ééé1b\.÷„72å¥ê’e9D ^¯WGëÖ­CCCƒf<¢(bÑ¢ER»€úþøý~TVVê”~C† Avv¶æs‰2AyLvœËòˆ„ì\DìÖèR€Ñ7Æår®þjȲŒ#GŽèlÖ@pò@fÊ-õ„7òÚ£=ÿ»?ð™€Bj"xë­·P]]­Ï„ °råʈ3ú(ß×NœŸ'D›7oÆ¡C‡4ã)..Æš5kBÒƒ;&t¤RÁùóçuÖ—Ëe¨c‚\êd`´ÿˆ½¾êëÙM @aa¡æM¿ß²Ý}éžžH’Ä̬˜íxWz³T^ÊÔæ>5ÐRÀîÝ»±mÛ6Íxòòòp×]wYªÞceRGò`´¶¶êܪA@qq±F㜈Ä ‰0¦hÏ¥·ÑnûèëEÛ ?pšúMzŹxñ¢©v¼µµû÷ï‡ÇãѼ§LH¥._¸•>\í>à³É¯Dü©ßSúW¤€#GŽà­·ÞÒŒ)==_øžžnéæÇR ØÛÛ«óF‚‰ê`¬D ‰>¦hÏM/@t¾ŒVI’øMsÐJÌ@ebÕÖÖêÌQjÔÖÖ¢¼¼\G===GµRŽŽÆcM|³É¯œÏ"¿ßšš¼úê«š‰KÁwÜÜÜÜ„ð‚Û¦êêjÝxòòò•ÒËʉ—ˆcŠäÜX:E‚hW@K:  †“¥ªªŠ™ ¨££¡ÉOO|eÒ‡[õÍBzÍ2555áÅ_Ô°¤ X½zu¨jÒ ì jjjt–•´´4 :Ôôº¼/Q&i"Ž‰÷Ü&€pÏU´>'œ>}ÝÝݺ›PQQÁ –ëëë…ööv¤¦¦†&9kâÓ“Þ,o?=ù•mKBèêêÂßþö7ÝvÙ²e?~¼í ?x‰¥¡¡A÷pBtn¾—óŠï1tuuiÞdá°óY‹Ö„!å *\=x¯×‹O?ý” о\WW÷ûÔÔÔZ¼çøfÙ{i‰A9_=¶×_]ç>{õÕWcæÌ™1Qø±Àócwttè¬%‚  °°ÐTéw9¯ØñSGG‡î÷¼d¶¢(Aèt:QPP h¦'Lww7>ýôSVåŸ:«÷ïß¿×áp¬òù|÷¨E|£‰o¥§€µò+«?M²,ãwÞAmm­f`“'O¶”ÑGé+P¾[__³ÔwAA¦I¢L<+ç^jcJõ3iç@èt:áóùtÁ---8zô(+õ÷!7I’t¶ ¿ðûý_ðù|–r/ÜäGtl¿rþ®]»túˆ‘#Gâºë®‹Ip™ÓP ÀùóçucÊÉÉAFF†ég/§‰7câ9—ÞæÚéBc瀹ÿ§WÿÚÚZ£rß|A’¤ñ_–åONçó~¿ÿKáV}3‘ЧùV+ÿÔEA***t9†ŠU«VA„„ ð‚÷’6—¦¦¦"//O×÷`•ã˜xÎ¥=Z3220nÜ8¤¤¤Àëõ¢½½]£Ç‰•ô¨†VôõõiDŒêêjXÝÿðV)¢@ ðï>ŸïN§Ó™f´âå´+¿Ùêïp8pöìY8p@óùÌÌLÜrË-as¶´°¹¹Y§P"„„¶]êqÅ{’Æò:ƒeL§OŸF}}½æ½¬¬¬Púx‡Ãììl :n·~¿ÝÝÝÌØ;a Ež½Ê"%%…;y kLJÜJJJ ÒÓÓCDâõzu:!3Ø!þAÐìÿ³³³QSSÃtKð€[%IÒå!2ƒ,˯:Ži‚ ü¯ØO‹þôê§NÒÝ´9sæ`ܸq1Sø)ã´Š‹/êˆ*%%¹¹¹ 9Abun"ŽI ǃC‡áСCºgÈápÖȈÊÛápÀív#555dÖV<ú|v˜ hžæsçα:€çü¿öÎ.¶­¤Šãÿq®?×NÜÆVšÄŽ“´qì(J4Q·I#Zª ØeßøZ!ZxàeY, â…¨Újµ‚*Pl•jUI‹ZÔ”–¦ÙB«ÒVM ]m’u…òѤI_^›ë{¯û:½?)J2×wf’;sιgfÎù c,{1%sþÆÖÖVò’ʵä÷t¦B$þA ª»»[S}¬¬¬(úZSS—Ë¥‰A¯ ‚p8Œ{÷îáÑ£GŠ‰ÄŸWsssÊ¡¬RèÑh„ÅbIú…!BÒZ,Æ )‰øT&?ð=ÆØói Y çœò…­­­k„^5ͯ@mÑòò²â½Ìëõbpp°¨f.’>`ÙØØP=Fêr¹N?-OB?«Å>mll`ffPÌ÷¼-€[®¯x…1öv>•ËᜯB>‰DþÀ-×üéL©ößÜÜThÓ={öàðáÃÉûjªj-D£QÅ;$_C&„¤V‘~—ÿ,ý;Ô®ËïW»W­ŽíÚÉTO¦þ–«lÿ/ÑhkkkX]]Mùzúôé¶ Ãn·Ãåreåô+5‰`°^¯çÏŸ—_VÆÙÏ1ögJé¼ @irÀ‹Œ±¿ç×]u8çÿ!„¼‰DÞåœ÷|T–Ñô—¾÷Ëͳ]»vaddD5{O)Ìý\4ÜÒÒ’jPG»˜L&8NØl¶ŠïùWCÖ@^yâ`ŒýšRzÀg¬ ¾Ì÷;ÆXz»¨8糄AÎù(çü¥L¦Bû'¼ÿR, FFF’k­ÙP.« ‡P§z „Àjµ¢¶¶V5ͺVØÚÚ’Ÿ!Üɧ®¤]Û0UXײ‡s¾Jy9‰üsþÝX,FÒiÿDþ¿”Ž Ž=Šºººœ4}¹œ€v»=§¬I:•CX­Ö¤^«?ÁãÇå ïc,¯M}±áñÙø!äv,;‹Åä“ßl6§„&â“mxx EsúÃ2¨©©Aww7ZZZ ®\¹¢Y§ü$N¥òÿôrÒÔrÒìÒZçÖ-…Ûî=µÏeCå=8çoBþ$ŠâwDQü†(ŠQa2™àv»y``MMM…ñVéCÞ÷îÞ½ÝÝÝ)‚JE:tj›©tʈ 0›ÍX[[Ãøø¸b;zµ!Ïe‰ÌŽüŒhBç| Àk„ã¢(þ(‹}¾¥¥…ȷȃAttt”Åá¤,ƒ@ ©í¨Z#F£Q±Í7¥ú;žwb±¬V+Ž;† .Tº;y³¼¼¬–">oG½f@ÎùcJé—¢Ñh« “^óù|èéé)ÈìÏÕROÈúúz„B¡”ÌÁÕâìÓ…KÜ»?22‚K—.Uº+y155%ÿ¸–o}šñ ùäw»Ý ”<ˆó¹ß`0`ÿþýhmmM»©“- £Ñˆ#GŽ(òDT“““ò¢3Œ±¼5æ¥ôu_“–9 pÎõ%; P áÒß߯¶¾®Þyç¹BÚp¦Ðz+.(¥ÍÆì’–÷õõ%ËæKº{ëêê’ïö‰ ÅåÐö;EkîTz{{qûöíJwCÁÜÜœÚÒ囌1EFž\©¨ ”:Ÿü{¥åÝÝÝŠ>ÅÀï÷£­­í¹ÔöºðÉŽP(¤ˆ,]I8ç8~ü¸|¬.ø~1ꯘ ”š7ûS2û|>´µµå<ùÓ™û6› Á`N§3%3±Nù¨6ápÿþýí?X&&&Ô6þü€1¦ˆ’”RàWŽHË=@^F~Ïçƒßï!$om_mWGI>ÏPEtvvbff¦=Êž'OžàÌÅkþ¿œ(V•²~ à³Ò§Ó‰žžEP\°X,èééÓéD4­j/¾.|*K$ßï¯Øa®X,†'N`}}]~é›ùDÿMGÙ¥ôU¯IËl6úúú}²]ÛoiiA{{{ò~yšmÜÐ…OQáõzñþûyÛÉÎ9Nž<‰;wGüÏ0ÆŠz¡¬€Rú"€ŸKËL&úûû!BN|, ‚Á êëë³¾¡lí£µgôìÙ3477«&q-£££¸zõª¼ø=_-v[e”Òü@òüeMM z{{a6›³vÌíÝ»Im_Hr…b¢µ«“éB¸y<„Ãá’·îÜ9LLLȋȇçË>µP–”EPJ;ü€5QFA(‚Ýnßvò˜L¦¤¶D"šÝš[)táSzVWWÑØبȖ]LÆÇÇqöìYyqÀËŒ±Ç¥h³ä€RÚ`@J:•}ûö¡¾¾>£æ÷x<èììL&JPqˆèT˜çIø,//£¡¡A5Òs!¬¯¯ãôéÓ˜šRÈ÷*cì/EmPBI¥´ñÔá~iykk+Ün·êäÁ`.— ›››ÓöÏÓÀ®fÊýœ–––àp8Š–ùwvv§NRÍ àÛŒ±·ŠÒPJ&(¥5~`@Zîv»áõzÎápÀï÷Ãjµ‚sžS&Õˆ.€´ËââbNQžÔàœãâÅ‹Ss`G|™1ö›‚É‚RZ'|JZàt:ÑÞÞž¢ùM&<šššh÷èíNB.…#Š"æææÐÙÙ™Ó}ÑhÓÓÓ¸|ùrºåÅEÄóo–%Z )Å` ”~@Êzemm-‚Á A!ñüê^¯6›­(mꃺ:ØiÏi~~@@hTþµ¾¾Žëׯcrr+++éªû7€Of›y»”ÊxEú‹ÙlFWWÌf3S2¬Vón=);m`ïDJñŒq÷î]„B)GZÀ9G8Æüü<>|ˆ›7ofÚŽC<3×댱²&“(º@)µø/$K~]]]EÕö:©è§òlllÀb±`aasssXXXP¤®OÃ_|1¦ˆõUJõ p@ èëèì>ð-£Œ±ŠIðR½¼àg%ª[G§Z\F|GìJ±³/WþñY@üù¯âIEND®B`‚choreonoid-1.1.0+dfsg/src/Choreonoid/icon/choreonoid.svg000066400000000000000000000677451207742442300232730ustar00rootroot00000000000000 image/svg+xml choreonoid-1.1.0+dfsg/src/Choreonoid/icon/choreonoid16.png000066400000000000000000000016661207742442300234150ustar00rootroot00000000000000‰PNG  IHDRóÿasBIT|dˆ pHYs|4k¡tEXtSoftwarewww.inkscape.org›î<3IDAT8]“MK+g†ïygæ$“˜3Ñ­m”œÈ©¶)‚‚ -ؽt΢í¦ÀmñXj€!+A«”RŠ°bô Q)üŽDLFœ ùð§+¥Ç .xV÷ 7<áÿ¤R©o‡‡‡À<t]‡®ëðz½Ï·ªª899iær¹5é)`ll,ÀÊd2Ét:ý{WW—~yy "çœs¸\.hš†››tvvB–eSyjI[–ÕÙÓÓÓÍfadY纮Ãår=[.—Q«Õ „Às@"‘èÖ4­³P(`zz›››0 www°, œs¨ª Î9šÍ&êõ:jµZU""H’Ä666ªÕªÇëõbyyÅbñzbb¢ýââ---ðûý¨ë:þ‘ˆÉdòÓ¥¥¥ý|>/E"8ŽSŸìèèøsîRU²,CUÕgEÁÃÃÃo ¥„’ÏçC½^G0Ôúûû§æçç¿Ç ¿Îf³+ ÆØg D£ÑÑJ¥‚`0Û¶aÛ¶³¿¿¿599ùN¡QA’¤·¿ú|>i||ü«@ 0ND+ÐÞÞ­V«Ð4 år¬©©©€"#¢7þ"¢/|BD¯‰hGQT’ɤgnnîÕýý=8çˆÅbX__?uÑ¿Œ±?‰è#!ÄŒ±´ÂÀÇy `E1 c$pY–ADXYYÉg2™ï,ËúEQ”Ô‹ñ&Eªª`ŒQ>Ÿ?Uâñø;Ænoo±µµ…ÕÕÕ÷ ´µµ½* p»Ýðx<Øl6á8ÎÎÎ’JkkëÛíím§X,²T*…X,ö®ë¸¾¾†ã8B Ñh@’$J¥J¥úúúpzzúž‡ÓFkkk‡Ã¸ººB¥R¦iÐu~¿¡P‘Hš¦!CA{{{9eww·ûèèè±··÷óh4ú& Áívöm0Æ Ë2LÓD¥RÁùù9¢Ñ(ªÕª àP9<<´fff~ŒÇã†Ñ4MS2M/ßü Ƙ(•J\.÷ÓÎÎŽýG‡t¥±i IEND®B`‚choreonoid-1.1.0+dfsg/src/Choreonoid/icon/choreonoid24.png000066400000000000000000000031471207742442300234100ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<äIDATH‰–mhTéÇÏ}Ÿ·ø23Æ1›Ø¼¸nªÇ"R )‰ÁÖEt³¥$­à§RÚÒ RYÜEJ…¦°”R„¶´ý’EŠKXD ÖèÆYÁ·FÛ¸q’IŒyïäf&É{Ÿ~h2˜V¥ç.Üó¿÷îyÎRJ^eÉdrS{{{vãƆ!LÓdÕ ÃÀ², ÃXsm5{333Ë/^|O¼ŽàÔ©Sw;¶k||œ©©) à `YÖÿÄUüüùsؼy3}}}cÚ=µ [úûû?Bì«©©É¶µµ}±ªªJ‰F£<|øß÷)‹äóyTUEQ4MCUU|ßg~~žºº:b±Xd ”rßþýûÿÞØØø뎎Žï544påÊ:;;‘RØ°a†a ëz9¾ŒÓé4žçá8 r A]]Ý×öíÛ§¬[·î»¹\Žƒrÿþ}&'' ‡Ã„B!lÛ&‹½VÖ¥¥%Ç!‰0;;[\C°mÛ¶C›6mBQ%NFÙµk—/_¦µµÇqp§,ªªk< •ëÙ¶=¹† ‘H$***˜™™¡££ƒÞÞ^"‘<`÷îݾ®ëÊÓ§O±,‹`0XvÓ4×ä–eËåþ¥¼ÔàÍõõõp8\&ìêê"NÛ?îïïvçÎåR©äzžçº®ë.--•ŠÅbiaa¡ä8N)ŸÏ—lÛ.åóùÒðð°;44”*¿âK‰DÂeqq€`0È™3g‚žç=º{÷î;@5 Voù?ð?ÊÍÍÍßbbb‚x<Žã8D£QÖ¯_¯ïܹó===}@þuÍM&“G"‘¯]»¶¸’ÿø ,QUUÕ—UU¥X,‹ÅÐuÑÑQ¤”ܾ}ûϯ)ª¿”þjµøŠ}pµy”sçÎmô}ß÷W%Ãq&''Ý|>óèÑ£ýRÊKÀGº®ÁuÝ_lÙ²Å8|øð,ðQuuõo:Ô-„ø p;oìëë³5€½{÷¾][[kNOOcš&ðŸÏQÁÐÐÐR[[Û„aMªªþ¶T*} (J‹çyúÙ³g¿}úôéÏßK)ƒRÊn!Ä€”òc)åûÚJ3¿Fõ™™,ËâÉ“'ÔÖÖòìÙ3îÝ»7ÝÒÒÒ"¥¼-¥ŒÓBˆ”Ò>þü[ÍÍÍã@Â÷ýQ JJ9áûþV!ÄD™ ©©é}MÓ˜ŸŸÇqQU•ŠŠ xxøBgg§üQJQå¾ï |SJÙ#¥<)¥ÔEùDJùOß÷˜À_Ë•••ÍªªbYÑh”B¡ÀØØXéÂ… ¿ê9yòä÷u]·Ey@Q”Ÿ¯à¯¯D€·ÏE©X^^^Îf³A-™L?2 MÓ°m›L&ãõööþDJùóöööû­­­MÓÓÓâåsÿM¸P(0>>^:qâÄ'š”rO"‘0'&&ÈårŸúÌôéêS§«ë<ç{¾« Ë2’0†(Šwx-ÞãH" txÀÓ’$UXù $cˆ¢èpÀÔx%‰$8 xÀ£’$µñ| I&EÑ   §Ó‰™3gÂëõBýUÿ/‚a»ºÍ®sÌÎÕ¸ÌþÆúš—Âwà=NŸ>ææfTWWãìÙ³ðûýƒJ7K’TîÄ$„(ŠGLQ^ãß@ @uu5ÒÒÒ4?¼ò¿•¶H?þc=ÖÁv?â~¿ÿþû(,,„Óé„Óé„ßïÇÙ³gQ]]ŠŠ ìڵˈÚÜ#IÒ?Yo*H@ˆ¢ø|[y]\\Œgžy~¿ÿûßÑÑÑ·Û þL’âÿììëÝwßEQQœN'Gè¯úÿúúz¼ôÒKرcs9àk’$=K¿¡ÀaôF!ü @ŸòâÂ… رcœN'Ö¬YƒÛn» ²,#ÄqˆI\jظq#†ö¼ââb<þøãøío‹«®ºŠ~ÛàQ}>Ia IÒQ·­[·@²,#;;÷Üs®¿þzäääÄgI\RØ¿? -}fìرøÉO~‚/~ñ‹ô[N/‰¢8”õ¹$ðá|Ê‹Ó§OcïÞ½e9täççcÅŠ¸þúë-ÿxI$¡àôéÓQI“k׮ŗ¿üeº¹Ô"¦ I$©À+ê¶uëÖi@9rrr°páB,\¸0)$a UUUhllŒºŸ{ï½>ø ݼJŇéÆ$ðã?´³Ž?ŽÃ‡“† †¥K—BE¤¦¦ÆkÌI œ8q–ɯ@Ù–Rø‘(Šéê†$pB’¤£þ®nS¤€M0räH,_¾S¦LËåðq'‘ø8~ü8.^¼h{¿>ø`ÈBÕ¡îS7$ À~¦~Q^^ŽÊÊJSe‡ãÆÃÒ¥K1fÌ8ÉÛžDÇŽ‹Éä€áÇãóŸÿ<Ýü-QC^FÉ'Ñ$IÚà]uÛúõë5+¾ Ȳ B¦NŠE‹¡¨¨(_!‰,Ë8zôhÌ&¿‚µk×"77WÝ4ÀÊ‹$XÇOÕ/öíÛ‡Ó§OkH@¤§§cæÌ™˜;w.òòòtðIIJ,ãÈ‘#hjjŠùµ233±råJºùÊ?I°I’vØ¡n{ã7 ufANN®¾újÌš5 ñø:I 0dYƧŸ~: “_Á‚ è¦%¢(æIˆ]ÀîÝ»qáÂ…ˆH@ñ!˜;w.&MšD+m’¸„pðàÁü0cÆ dgg«›œæIˆ’$½`¿òZ–e¼ù曆\ ³sŠ‹‹1wîܤ¢ð„ßïÇÁƒÑÒÒ2à×v:˜7oݼH@4ÐèvìØÆÆÆ°«=`.8”––bÎœ9IEá%ŸÏ‡ ¹¹9nc˜3gÝ4H@4ø;€Pö¿ß·ß~;,¨%³s\.ƇٳgcÈ!ñø~I؟χ}ûöÅeåWcÔ¨QtÓQ]Iˆ’$Éz†°uëV´¶¶r‘¯~ -- “&M´iÓ™™¯šD„ðûýسgZ[[ã=Œ9’nJ0-IÑáeÕÊ ¯×‹7r¯~@–edeeaêÔ©˜0aBRQ8àóù°{÷n´µqeæŠ9òòòX–¦Iˆ’$ùü—ºíƒ>@gg§%à• 773fÌ@ii)œNç€ç$ÂÃçóaçÎ 3ùŒ1‚nÊO@ôxÀåEoo/6oÞðn ??Ó§OGQQQ(›LñG__>úè#´··Ç{(: ªK $€h ‚{ß¾}…]]]ëÕí›7oFOOOT$  œÅ °°S¦Laý¸I 0z{{ññÇ£££#ÞCa‚±Px d0B`¡ ·8ŽE‚ ”¸\®|A„3gÎ`âĉ!‘¼«« ~ø!V¬Xa˵iR AAII òóóQ[[‹ÎÎNø|>´µµÁçóiòÍQßÉ´Ýê9fÿóô5׈¶/Öÿ)))ÈÎÎÆÙ³gáóù@HbN+ä'æH‚ d¸ÁápÜìr¹V9Ž<%)#б­­Mc®{÷Ýw±téRÛB€Ã‘¸Ýn”––¢µµ[·nµåºIX‡ËåBzz:òóó*ù,wP€(ŠÅ’$ÕÅ¢ArAø!äN§3ÎÊ*‚Žz{{!ËrˆeÛÚÚ°cÇ,^¼Ø¶qñ€„A/x½^´µµ¡½½yyy(..Ž÷0%€¾ACý1ÌOX `€nQà—’$õØq ARAxÔår}ßétæ9NB4“ßét2 @!´´´P›6m¢E‹bâÖkF)))¶_/ ëeÍÍÍèééÁ¸qãâîÞ͸~Û !Pç4KðC÷ˆ¢8O’¤ˆý,Ap¸òCBÈB”ɯdPçd7ÚГ²©© »víÂ5×\éЂEC‡Evv¶N=dÈÐ8é½,«îŸem ÷ÉJ?FýõnÏ®î—ç|õç¬|_̾·,ËèêêBkk+¼^/óó===8uê&Nœhxã{ Eq(¯;&xEÅ’$…­™DC„<‡Ãñ!d™Ëå!$t(Ÿ–Œ€îÁÚ´iæÎSS‹ÆŽ‹ƒjÚ†Ê? ó¥ö:VÃ]·££---8{ö,Nœ8¡ù]º»»ÑÒÒ‚‚‚Ä Œg°}°˜WiÓZÖå0&C‚0Ñét~âv»—¥¤¤Àív#%%Es¤¦¦†þò´Ò¯®®ûöí‹Ø$©Ù°¨¨H·¨«‹‰Ê$‰~dee¡´´ .ÄÊ•+™ÏB<ÁÚ Є2]sÍ56l}ÎwEQ¼‹·CA®w¹\Ÿ¤¤¤Œ§'==ùé6³#--M÷ÃoÚ´ @x»È@‘BJKK5ãèèè@WWï-J" êBp{{{ãpúôiôõõÑo ¸Zýb„ ¸þúë‘žžNŸ÷'Q¯×™ ßt¹\ÿt»Ý9n·Êa&( ÐA;gÏž5M!+5j”Î]8Þ«Ð儲²2Ý3Z__óëz½^ôôôàСC8|ø0.^¼ˆÜÜ\VPRm€(Š£„Jí¤¤¤`êÔ©ÈÈÈÀõ×_O?àéþaT A¸ßívÿÚív;y'¿Ñk#ÈÈÈÐýð›6mÒLÎ ÅAH¦¦&x<žè~”$¸àp80}útM[GG·97A€ßïG__º»»ÑÞÞŽ¶¶6tuu¡¯¯¥¥¥5jT(x¬¡¡î¢&á ”ø?vìX! °° .¤Ï àUQu‘2‚ \ãr¹~ïr¹`6ùi"àÝÿ«Ú5·ªªŠ+…¸Ý$0zôhò') [ÕˆKɲ ŸÏ‡¾¾>tvv¢­­ èéé×ë5íW–eVⳃŽ{joo/`êÔ©˜6mý™¥ ¢ôAIyÃÜn7h"P‚™N€‡²³³‘••¥Ô;ï¼cª¸‹Å‘––¦Ó<766Õ”OÂf°Ü‚yî½bªôù|ºÕ½··>ŸÏ’$ÑÔԟϧnj”$©gPÍÕ Ïþc¢(Þ‚ ¤9θ\®á.— ÊÁ"E0"+$@{€;vL“BœÞ³Çê=z´f~¿Ÿ%&°€ž¸Š¹Y–eôõõ¡½½---hooGWW<OÔÛFÙ±³@‚§E‘˜­n?~¼N¤u:¸ñÆYsžEq– ¿w¹\³Õ“ŸEôÄWÿOïýy¶C‡ÕUKE999tVXÔÕÕÙ¶MÂ,P|H<ÚÛÛÑÔÔ„ææf´··‡\Êíkÿ$8˜† b@0«‰pC›Ú222°jÕ*Z)˜6¦¥¥}‘vⳈ Z}½ú–——ãüùó†$ †$@›=O\“T^. ÄnÁ8‘ÖÖÖPÈø@`°SüÀ2¢¨¨K—.Õ´9ŽÂÒÒR¸Ý$ÀšüVˆ %%………)@–eî„!ÊùvºÈ´ . ‰Ø¢³³S×"±Œ-Àà$Åõ’iL›6 3gÎÔ´)¡™ÊD7“h}€RÀ„ 4ã9xð W q;IЧ„êêêJÈÌ5—hˆW®€KB(++ ýŸŸŸoø¡%K–èö¬¬,dddh&?ï¶ ) 55¥¥¥ xÿý÷-M^;H ¨¨H÷ÖÖÖFþë$thv¼JÄ:E1À$åµ 0+šáp8póÍ7ëÌp™™™\[pD`䘖–ÆÜ(ÎKjìÝ»---–H@A¤àt:u÷­¥¥…å"š„M  )ðã*¨ÆWRR‚ÔÔÔ 33Ó”M322pÛm·é\aÕ &šÂYxL‚êsƧ!$¿ß?ü0bq>R())ÑYQ’R@ì@oâ!(¦E<ê€Ä&ÿ¿Z¨èh¥¢¢"f^¾@ `JŠ.Àˆ¬ˆÿêó¯¸B¦ðÉ'Ÿ„\CŠÜn·Î1¨¡¡©­N"z$0Vÿsý…mš - ÔÙwŒ0cÆ ˆ¢¨i“å Ã‹xtB__߀’@^^^Ò1(Æ “¯( eƒN€ý?Ò-€‚¢¢"¬^½ZÓæõzQ]]Êù§&–~€ÞðN|õ1kÖ,Mº®îîn|òÉ'¶-rÆ„@›»»»¢’í¥‚D1‰K €ÚþÉ@+®¸B—¥·³³SGêÃH7`&Т¿2ùN'ÒÒÒ0cÆ Í¶o߯×k ðHº‡2)؇DqT[€þÀ+Õm, €bŒD¤ºþúë1vìXM[mm-.\¸Êø«® >X[3ÅŸš4Ôd2{ölÍäëèèˆiòP@O‚ `øðášû ©$=Aeyp¨ÀÜÜÜP>{‡#")Àápà®»îBnn®¦½¢¢mmmºüÿfD`ä!¨^õYõÒÓÓuÞÛ·o‡ßï) (PÚ† ¦»·I)À$B@[[]¯ E’¤ÐÀ‘ ýÿéœì@dÛ ¨¼÷Þ{5bY ÀîÝ»Ñ×תþC“MÊ$gM~õÄgÕ˜={¶F¹ÙÒÒ‚C‡ÅŒX!D—¾¬±±1™70Jøý~$•h&@`€5XUªQ\\ŒÛo¿]ÓÖÛÛ‹;v„Ddú`I„ÐV %%E7ñ ‰dddè"·oß>  &z’y£D¢ø˜‰ÿÀ !ÖÊX7²0sæL,Z´HÓÖØ؈]»vq÷¡Œb8V#̘1Cs~cc#Ž= `à‡¦¦¦ê§êêê’y£Ë€6eÂ@B•cEŽ3ê×4¢‘¬\¹µµµ8qâD¨íرc(,,ÄìÙ³M'P @ %iôûýðûý\033cÆŒAUUUèÚÛ¶mÃäÉ“ðWŽ………š‡ÖëõâСC¦¶ku»"1©ßSÚÔÒý¿YŸt«¦뺬1¨_]Ëì:Fc4ºí¬Hp[€„""¨@z‚Ó+¬àp8°víZüú׿֤ÉÚ¾}; 1fÌÍd¡‰îóùBIGˆ ”C!å`‘À”)SP]]z8kkkqüøqŒ?^3ÎX’AVVRSSCÙ–èâ$"G"šÄÛèjÆ«‚[ééé¸ÿþûCE€à^øí·ßF{{{h"+“ÞçóÁëõÂëõÂãñ ··½½½èëëC__¼^/“X„••¥sÊaé€Øn X9ì“° ä4x œÆýÛ…ââbÜu—¶¼`WWÞxã x<Í„VÀãñ„&=M‡IjPþ§•gÏž5L!Ćòòò S­%9A%³h * $5hPkìŠd›5kΟ?>ø ÔV[[‹M›6áæ›oiö•ýž"ò+{eu›ZJ I€– 233‘ŸŸ¯ ÈÙ±cFvÌvn œNgè^Z-›­ü5:ÞoÖw¸ó¬^Û¨_Þë±®­~‚‘#G"++ §OŸÖl±b ǃ¶¶6u“€&ûK€Q kågéì¬z{ÓM7áüùó8vìX¨íСC(**Âܹs5U ‘‚z« H x½^Íû Œ5JCUUU¸pá‚iú3v@kk+ '¥ò:’#šÏ&Âç£í#`ôèѨ¯¯§'eÌpñâEú¹8/I’Æ´“H[ÿii©fRé€ÈŒ î¿ÿ~]âÑÍ›7£ªª '$ú+b?}(Ûe+À"µ4àóù••¥Á?úè#ns^4â¿ÇãÁÉ“'m½Ih0|øp†?Ù„ÿÄ"K@ê6»j¤§§ã«_ýªN)¸~ýz\¼xQ3ÁY“ŸÖ¨ @½% ·ôÃQYY‰†††˜’@ ù$[ȲŒììl®­]´`¥–ÆŽËœð@lL,ã¾ûîÓ´uuuaݺuèîî6\ýi @MFD ééé:‰fçÎÜ“Y ÞÏœ³ª[G…ÁD¦öÿpz€´´4Ãm‚]HOOÇ׿þu S¼÷Þ{hjj2œø,)€v¢¥%Q©ú;ùý~[Ò†A8zô¨Ž’ˆ†ªËQ -YXPˆ "ââb<øàƒš±ôööâƒ>@WWWD A³-½7<|øp(Ì4¨©©P‡”$ø ËÁà0ºpK$hoo§s:´K’¤SöÄDQÌ0Yy-FÍ-þ+í±²И={6V­Z¥iknnÆîÝ»¹tf$@“+ ÀëõF6¬¥¥…µ2$‘@p»ÝQû ð¬þ@¨€ÅÅÅ3ˆ)…[o½U—Ú»¦¦•••ÜR€úPb XRmž;xð z{{#šü^¯7éì3Hàp8PXXq1à a @W€†Ùʯ –¦@Öu¿öµ¯é\t+**PWWgY 0óèííÕÄ9ôõõáÀ‰þGŽ°{”„=6lXD‘„ƒIÐYx€Ôm)A‰ã[ßú–溲,£¼¼mmmÌÉoÅ* >è8‡XJ!ÇO„eC‡Õx¤ò`0€¡À³ò+ˆµ)…ââb<üðÚñx½^;v ÝÝÝ\“ŸÇ4ØÚÚª‘zzzpøðaîÉ_WWG—‡NbA–eäääX’rÅ@Å‘Br´º <Ò€Ý6T^Ìž=[—X´§§gÎœ1Ô˜éÔ“_ù¿··W“©öïßJCfvôôôàܹsyK’ˆdYFjj*÷B7X$Íê?jÔ¨P¬=pÒ@<$·Ýv›®y{{;ÃZx¥€úúúJ¡<Ó¨¨¨KÉ ŸK .—K—À•ž@ Á@ídU ¤ãŽAÀ׿þu]ÅÝ––tttpY˜ëz{{WxVÝ¿™I@ÒÓï…Ãá0ÍÜä÷ûÑÒÒ¢n `Š EFXívç°‚´´4<þøãº}Z{{»fÕ'0È`Ÿßï¿J–å“~ äÝÑÖÖ†ÊÊJÐMþ¤³Ï¥¬¬,¦™‘¤V’$¦8nÀŠ3fŒe  æEQQ¾ùÍojƤìä£ä!ýÇk~¿¡,Ëç@’¤sžW_S‘ÔØ»w/KLâƒòlÑ5x€@|%M @:0"H_#\y啺Ģ²¬À’hË@ÿ!{<ž§ü~ÿ²,Ó•: ”Ö©¹¹§N ­þJ.ÿ$. 2iiišzŽƒ…Šÿ€y*0‰@°fÍÌ;WÓ¦ˆj¬}?Eç¼^ïm@àG¬¾%I: à5u›"ÔÔÔ$mý—!²³³1bĈPq^ `‰ñ´¨!¾ùÍoêÒ>¥¥¥A–e£¨ÁÇó¸ßï/Ëò?Â\âgBrcc#Ž;†ÚÚÚ˜GE&‘˜p»Ý˜4i***,IñÌ l‹úÜD! ¨|â‰'ðío[ãÉ7dÈtvv¢¯¯O©ÐãóùþY–.Ër+Oß’$•‹¢ø6€ÕJÛ¶mÛ4¢¿å©)‹EÄVûR¿¶Úo_¼×`ýoµO³>ìúŒ•¶šÝ|¯¢Ä…Œ"iXñèèèÀ¡C‡PWW‡üüü¸Zá»ßý.~øÆuJÇñãÇy½Þ7üN–åótÿ«Ð/Å)…F’H‚Â)›ÞŒ×@S°°°©©©9)X¿~=$IBGGÇ€`0ÕW^‰/~ñ‹š¶ÔÔT̘1ã,ËOF8ù!IÒ^÷"Xì!‰$Œð## ?ˆHü7Ò´¶¶¢¶ö³‚'²,ãܹs SÞúóŸÿ<.\H7ß)ŠâãÑô+IÒËnp&š~’¸$!xÀßÌNŠ— * ú=õäWàõzqöìÙr1Þxì±ÇBõþTøOQJ’d(¢…ƒ$IlE1€zß#püÏ{Þ@}&9{Ϋ—$é80`Å§@mU£«« uuu(..Žv¼Q#55O=õ}ôQtvv*ͯˆ¢x•$I§¢é_’$.bI¨1à[:Ðívk&h$@F"(¿è¸¡¨¨O>ù$í¾™àï¢(&Ž #‰ËñÐ0#iðêêëëM/xîÜ9twwG2VÛqå•Wâ ›§øËÀ&‰Ëñ M Ñ£G[Rü)í tÊ>Zä—eÕÕÕ ã%wçwbñâÅtóQ¿ñ$qù"î@´@´øïr¹0cÆ ]¯×‹ªªª„É…ÿøã³ÒŸÿDÅñO—'”Œ"iXÑЀœœ‚€Y³fé²uvv&L)¬””üô§?¥ãº^Eq\œ†•Äe†–¦)»rrr——•Û*‹€‚«¯¾Z—$¤¡¡!aBe ñÿñ´$À?DQŒOŽ³$.+ 4hÄÿÒÒÒÐÿ‘8±rå©K+eff⪫®ÒõY]]­6ÅÅW^y%~øaºy*€Çâ0œ$.3Ä•Œüÿy‰ ®®Nwž"((,,Ä”)S4m²,ãĉº¢ñÂwމ뮻Žn¾"cIâòÂ@;Y²° n§Åÿ´´4¸\.Ýg&OžŒ¶¶6œ?ÿ™Û½ÇãAee%¦OŸníÄ·Ür 6oÖ8NŒ×X’H|ˆ¢Xà«ÊÜ6 †ˆ×8àO’$…0è -Å‚ „Ý„“Œöÿ,Ì™3ï¿ÿ¾&?~GGªªª0~üxËßÇ.tttàܹs¬èÅø‡3&‘E±ÀaCLN»À#áúH @ÈÊá­À„,\¸›7oÖˆþuuuÈÊÊŠ‹»ð‘#G0räH̘1¯¿þ:ýöžÐe Q—ø&€1Ô¦™‚«ê~OJ’”Vþ?˜O~¸ FL`¤€¶¶6ôôhÓ噥J‚YTçÏŸmÛ¶iüNž<‰ÌÌLS±²,ãäÉ“˜4iRÈJQ^^NŸ¶{@“DQ\“˜y|hÇ IDATIáÅN¬xCZÊ@øâX% ¡0 zõw8\JJJpÅZýš,Ë8rä]O=&P*þ¨·?@’âŒ'8ÏÓ¹o4ú}i¦rœÚÇÓ_B€‚hÄÿÌÌLîRÊÓ§O×åëóx<(//YV%…3ËòÐÔÔDŸ>c2$4EQåœf‚Dð%/ ×tëÑÀÓÙ€€(Š#Àˆ´*þ›Y‰ÿ4.\¨ñ‚E<Ž?n©ôõõ¡££Ã0A £l÷~I’b/Ž$%à—$BXéðð§ô6rJЬþ#GŽ ­Ö‘ùý~44h Î*¸\.,[¶LWvùÂ… ¶Ó hooGww·i BRü+JßB"ø‘rž—¸PZZjÙä§ÆÅ‹u«©UP>³dÉ]ÿ•••hm.¿Foo/ššš¸ô I‡pÚt5áwü€²ÿ·ê¤¼G{B". 2räH»°,Ë8xð`D‰Eý~?š››ÑÚÚÊypì˜.{S"Îóêìå@81è$M`vv¶¦ +D@K<æ¿pX²d †ªikkkc‰ç:øý~Ô××£±±Ñ²!)þÇ’$ð5}Xð!èt‹$I‰LÂVÀ@8iüÿ#uRÚ{{{u9þ¢•€ RpåÊ•xíµ×Ð×÷™ õܹsÈÉÉÁرc™ŸëèèÀÅ‹áp8t¡Ç<8zô(Ý”$€†$Ið‡þÌÊjÓPA‰,1¢Æ‚°Uˆ(˜A˜ùÿ쀠+ñŠ+ðæ›ojöïGENN Bm>Ÿõõõèíí!„ÛF’ý™•=»ò ³€‚hÄÿÔÔTf`¤(--Åüùó5m²,cß¾}¡½zkk+Ž?®«Ënt†"‚’HÂ<@€®D0 1%Và¨Q£,ý Ûb±ÿ§qÕUWaâDm4n__öíÛ‡ªª*ÔÔÔØâ1xìØ1ÚRð©$I‰‘¾8‰„ƒ(ŠÀа'’$q•ÅŠµ ‰6lX(ì•·èÝF[bApà 7hD~ è²­€Iñ? ‹}¥ ¸L€@ì ÀV öövmÞ .— ×^{­®ÝÎzƒtS’’0ƒ­û öÀmà!Zü±+¨ÓÔÔ¤y©¦ŸY–“@IX…­ öV[-4dddD¬}ç]s€ŽˆçΣ•ˆÍNØv‹˜;wn ‚eÊvïÞ›HæÌ™S `‚QnùýÇ*².ʲÜ@Ú³gÏÉ×@BÅT©õm—bFý€¡4;t @ñv2C, €!þbÅÑdáÂ…EB0YE‚{C?€›wìرÉìó ,`iñÕ&BåRß‚ ª$£¶}ôÑGoóŽ‹óçÏŸ×Ý„îä§óçÏ?.ËòF¯ïܹs§cš7oÞPŸWgS€3»ví²• çÎ;Áû>N9!%„¹sçžðiÿ±c÷îÝšûît:• YýGŒ‡ÃQò ¸÷nlÔZ6èÂv¢««K“?°—¢ÿ/^|!äc¨<,)|sñâÅ+·lÙòã³Cü’ò%°J)&÷ðík¯½v7€;·nÝ•‹òç>÷¹y~J±œX£ßZ2¡ÿøÖ¢E‹^ðííÛ·G¡·hѢŲ,ÿÂJ Ù»páŸíرãÇÑ\,X ø!äVo½Kú|wÁ‚;üëG}´\.W!ž“Ä#:‡Uñ¿±±Q§€‹%ÐÖ æ7Bžƒñä‚["€åË— ¾Dù%(SÇ5ÀûË–-›ÿþûïsÙ—ÕXºti€?Bn²úYª±Þ`å’%KûðÃÿI_K–,É&„¼ ÕÞšº©~´xñâW·lÙQ¢ˆk¯½v"€§ !Ë"øø5v]{íµnݺõÏ„`ò»q[Œ¢u¢= !̤¢vÿ !†e½½½8}ú´ºIgЕ+WNp¹\³€°7T1eÅŠù„õ™} Lã<à»<ãTpà 7Ìp¹\dYiåsáÐ?Ö ¿¿îºëü›7oþ“Õ>!«` X£îÅt– `ùòå ].×? ‰¢&¥Àÿ.[¶,“ÂÔ0úŽ¯ÀŠT€¬VÿQ@@,W@/ØémxâÄ Ú‘¨’C „¬à|˜ŽÀM7Ý4„ò>,1éÿ¡U«V=±aÃ.ßøU«V­ „¼ *ãŽZeYüqåÊ•7n|ÍÊg !"Gÿg~=5V¬Xq'!ä¯n©èþ¬Bða“Kô÷÷-ÀT¨DÔ¬¬,äåå…ÞŒ$ˆVÆ’dYÖ]ÏNˆfÿO ™VÃïöÛoOð/¾‡ ™Ð Ù¾à;aÎ 2‚ç=å^|`‚äW… 9v‚º—Á7ñ•þ¸÷ÿÀ€ÚÈ ( óÿ«Á6 ˜rŒÜ}÷Ý9„ÓÒáýÔ?Cð‚AF?ðÓ—_~YÍlŸxç _øB€ßòŒA–å°± „ÿEp××S¯¾úêŒÞ_¿~}€_Ýyçÿ° Á‰a†¯ßyç¿~õÕW¹&,'ø\{€;î¸ÃMy ozª7<ùú믳’Nl€5kÖÌ°*“zp‹ÿÀÀÈ‘ZýU3 -Ž§¤¤Ø:!iÐ`çµè|à©ûÇq%øV–a²,+÷#Ö¾ð Lú} œãÄ€{ï½÷&BÈŽ~~üâ‹/N~5^}õÕÊ{î¹çA'‚R|À/Ãõy÷Ýw§B ԽôÒK\þß„˜Íqª à‡~ôòË/›îÖ­[·ë®»îºÁtdLéB|  ¿®½.†wàx+c¼úK¼‘[á€p«öRp¯øÎLÀk×®AùWÎ>ÿí…^øç¹xå•Wêî½÷ÞÁGqß0#­Šÿf€Xúÿ÷õõé2ÙIŒºQïÿ p÷ÿøÇ°+!D©.~˜œøÊW¾²„2Çè}¾ÿ¿ÿû¿–²ìüõ¯õ?ðÀLße†<ð€ëOú“iÿ„^³$ïvâ{àÛ«oðŸœ×V÷o» ˆHü7jgEÆR ·€½ ÀH÷ÿ€% þãw¿û݇œýŽáì³á™gž1t%„ð”Øê‚y>CB.rœ–àJiê*l§ðÐC¹\®€°Ú}À#Ï>û¬eÀ %^ ¥½úÇ:&Å}Ùx½^TUUÑÍ\ðo|#Ÿ2šóR‡TúqRþ,À£7}ôÑbBÈŽ>6?ýôÓ–íê@á5o‚}V „< ªœ»¼÷Ì3ÏTs^—¾Æà$»€ÒÒÒl›,Ðû§ÓÐ:fÕÕÕðù4ååNK’ĵgëWòâ…_ýêWÜA,„ÅÌŽkÔÇjôo#¬‚BxKså‡;Áår¸ìña%BÈFïQýÿ!\_FˆE`3Ѐ.— ……Ÿy/F’ l ÷ÿ»èˆ]8qBíË-þ»\®«® àe+ã²°µ0ÌWH¹™ncŒ5àŸü#Ó]Ã6`é î­©ðØc‰„É“ÑW-",,ò¯ÿú¯!DãaòÄUÐ$)))Ñ­ VR\¼¨ÝöÅ’ZZZ4)Á{%€(€šIjòlûÙÏ~vž·ß'žxBp¹\Wrz¦1 àûßÿ~–™ø¯ê»êç?ÿ9WÕZ\.—›êÏ<`¸ ú7%BÈfï«úúË/ùˈª B†À ‚ã^ÄÕ ÀŒŒÔˆU0–ÀJ9n'D#­ÒŒà%+cêƒÍ1éOA7]Ãþ>®‡\–å¨J/[ˆ† û\óèdYëDY®Ÿ~lãþøc¾ÁNˆ…øOC–eËÅ+y‹dY¶ƒb"0Žlq²`=‰xõï¿NLL€€½ K¦†U"ðxàŸš½i—àr¹Ò9ͧQ•“²@– Ö–Õ8Uy-‚Æ ÄRxæÌ|üñÇ:£ÓéÄâÅ‹1qâDȲŒÙ³g#%%Ÿ~jú<…C‹Õÿøƒ>hi‚ÙäÄK¶úík_(@è¡ CEq“4€Ls ½°‹ÊËËqèÐ!]{jj*n¸áiòõM™2.— ‡ŽøšD1Sšõír¹²9WÁ"€°lm£€™•–q?¢%€„×0íÿ‘:µ··ë<ò¢%€@ €={öÐÙxyyyX±b²³³u?ž,Ë(++‹Šˆ,íÿŸþy7!dF´}Ó€ D¥Pq¹\WpMXàŒð#ÌÞ݈¨ú˜þüç? .—«€êωA, +â¿ Q¥ïëëÃîÝ»uf>e¬Ë–-ƒÛí6-ù=bÄB,oºººè#?,LV—Ë%\?¾%  òóį·Ü}÷Ý: ¦ª¦£ c¼¼ŠB&œNçJÎS·›½ùâ‹/¦púÕ×®]»Ö4Q !¤‹ÕÎè/⇗ålG=€PÿŒ‰…°€h€”„"‘ ££»víBW—þ·š84Ö­[WÆÒ°3Æêp{P°D,¦ŠEBˆéC§êŸÇóŽ‰W_}u¬ËåšLõÇB;‚IO Î HÕ?O°ix²ª/%®Ñ5 ÷ÿÔ½h¾çž{¸Ë4ýG2(5DQTÊø,0 š"]ý!I’.ï ˜3g¦N ê-ÍV~C† ÁŒ3X•}˜ˆ¥ùOõ¹õÖ[Ã&ë¤úÖíÿ &—©´Bá î‘eyßȘ×XÅèuê?׬Ycšk€WÎ °¿/ÞtáÑ@Øýÿ½ˆHÇb‡ ÿ‹‹‹5tV‰€ÉþÿÌ™3(//×=(.— ×^{-FŒa8ñÃIÙÙÙ˜:u*+ºO‡Xãá·´ÿ7ë›Ñ8hD0Ì7œÛdæ[o½å^½zµõUŠA ¨±®çèËÎD ¼ŽM¼Z|Ö5b¦b@t^=@SS“nbZ‘dYFEESÙ—‘‘eË–!//ÏT”äÙ¤§§câĉ¬ ?š~"Í„×Òóøé³°iÓ¦tBÈÔpçõ÷oÚ÷êÕ«ý7n¬VK/ËòÕ>â'üóŸÿœÉ“iH–åó홂—8%^¿ ¶W,$‚I°b÷WÚÐâ¿ÓéDJJ ý&ü~?:¤«" ùùùX²d RSS-‰ü¬1*HMMŸqãpæÌ&iÔ××Óº‡púÿÿþû^WS«1ýl<~ÎË—/»ÊB6x”£¿û`Þ}÷]ò;ðõ§×_½Þ«‹‚Í/ܸiÓ¦´+V„Ÿ7ntB–sž>ðÀª¨¶DâDO^ÞðßÞÞ^>|˜é¾[ZZŠk®¹N§34ù­*¤BJKKqá±0VÿO$Iâ²eB&€¯,–Á€Ü°`ÿç’,!oƒ“>üð×,Y66þƒ>pBþ`G¿§üžã<[³¯X±¢þ½÷Þ;@öªE€/x†óÚؼy³“k NçüH\$€)Bî™™™™š€ ¢±ðˆÿ(//‡Ç£ß^NŸ>3fÍÝêIlgJ‡Ãââbœ8qB“A8JñŸ×þxÁ‚–öÕ1 €mÆà‡ó tX¿}ûö»-Zd˜xÛ¶mã!ÿÀjÎqþàsŸûohµ­õ!ðÇ©ÿwË–-/^¼8,Yøá‡Y„߸›g ýˆ 0íÿ‘:õõõ¡½]ëÍŽ.^¼ˆÊÊJÝêëp80gÎŒ=š[ä&Ù¨ (,,Ä™3gPTT j°u’R}Ûê\´hÑ"ÏÎ; à¿8N àÝ;wnG°^€ójÊÇȲ| !ävð‰ýðâüùó_à<——Â:©úû=‚5 ÃùT¤ؾcÇŽŸøíÂ… 5ÖŠ;v8,°ÖårÝ€7Î@A hÅÀœÎ;‡šš]{JJ ,X€üü|K+}´RAff&JKKQSSƒ‚‚œ?¯É!ØÃÛW¬\€÷îݛݿ½à7¹B~ ˜Ë›`QÿÁço± ÀW8¯‡Ý»w»9«jçÎËU­iáÂ…u»vízÀZŽÓs¬Zôã]»vU!X¬5@!!d€\ âç0.fÀ°+zš\.3&_–eœúè#Zò¨”$‰7«­¢¨ãÁ'VÆçr¹fɲÌó¥ÎÍš5‹«(&\}õÕÞýû÷ÿ ‚Ž8¶UR1ùýN¸åÊ+¯ä®1àr¹†s~wKy \.׿¸N–e^s_*‚DiJ–ýU‰y·,eŠ˜X5GŒa©èÝÎò¤áóùPUU…ŽŽÝ{ÇÇܹsár¹"¶ñÓˆ„@š››±iÓ&ºÙ´P…GŽÉã 5­6m¿["¢ª.¦ÿw¬ô ³gÏÞ~øðáÛ¼ªPF´ Æú€û®¸â K>ö„9)Ìw·tOEQ¬9xðà-¶ ÿ{Û cÚ àNg8újž={¶¥Tp ¢‘D¨öiùùùs]$ž€´@‹ÿ½½½¨ªªÒ @YY®¸â ‚µß ᤢ¢¿ýíoióŸ+B4©LÆüoŸª¾s8ï¥ì f̘±áèÑ£+üá•‚aAÕ àßü×Ô©S#)¯¥ó–4¸–3øÎœ9swyyùýÞ·ÐCáóö €û,ëÞ‡à6Á¬?îÅ…F4 ñÿ§÷ÿ x‰€¨–:;;qæÌ] ¿ ˜>}:ÊÊÊ"ù­Hfýoß¾Ï?ÿ¼nŒ~#IÒ1ÞkLœ8ñÜ©S§ªh NP×>Šàd°BÈM‡n“ïòÇñãÇo±Ú·‚)S¦l9qâÄ8Y–Ÿð0Tp„èð¬,Ë¿š8q"—vž…©S§^¨¬¬üÀ|ú=Õ½øÁ hÓ¦M{åرcÇü·,ˆú ðÃÉ“'‡2;WTT¼àúDÕx;ü ‚kˆŽt @+E?è6zõ!DÍÍͨ­­ÕM>BDQÄðáÃ5"ÿ@ˆùôç×­[‡wÞaJÍÏà)™­ æÚÀ|–lUF°:o…,ËÏ=Úr½qãÆí=}úô7"è½çêÿþ^÷‘ÛÆŒ£Û¿XÅøñã/x¬ººúײ,À ^ý@3‚NC[ àÍñãÇkFBÈWÜÌDЂ"¨>wF–åç'L˜q®E!Ò‡_ÅsPýË¿ü FŒ§Ó:‡æµYûž={4é·RRR0zôh4662sö¥¥¥aîܹÈÊâñ•±+Ò×ׇgŸ}ê‚ñdÿ.Iw‘ÎK555:ö” 8ù† ¨ùîCp·ôG”5Ê>g8áôéÓiƘ ËòDãš48`Ϙ1c"ªd"’Œ"iXQÒ€ÛíÆùó癞}yyyE)))1±ñó’bkk+ž~úi–)²À}’$½Î}ÑË£FêB˜pÝK £GîA0IItyåb„H·ñ¿¨¨(TF›w«ÏõûýhjÒFVvvv2'bqq1f̘ÁïÀnå_MM þçþ­­:Et€Õ’$YNÑ•D [ ¤D›cÁj +5aLJñãÇ°ÃÏB4z‚C‡áÏþ3Ëõø€›$IŠXQ•D‰H ¬ÀŠË¡G ‡ÃiÓ¦év²úæE¤Á{ｇ7ß|“õù ¾ I’=Å’Hb`™X5£ub¹+p»Ý˜9s&rssÃNÚXÚøý~?^yåìÚµ‹õöøŽ$Iщ%I$1ÀˆDÐDfdd 777ô¦@ê¾gÎœ‰´´4ÛbøÍ`D ÝÝÝøÓŸþÄ*ïåðˆ$I°<¸$’HDBLû? ^"e™©é:t(¦N BHÄ6~;$‚ÆÆFüñÔ¹)#˜êùI’.+­v¢@Eõƒ`ô¿Ù{—êÿ¼Ig@QZuRÚA@nn.ZZ>‹“1bÆÇtëÈàž'Nà¹çžCw·Îƒ´ ÀV¼ûŒ Šâ÷"IW¬z+¦Dû? ˆ¢øgß–$)l¬„mX³û«Û¯¹æšPô\qq1 ¹Ýzcåõ·gϬ[·ŽåÖû€[%IŠ(øB Qoð*¢w•M" 5¾ `…(Š7J’tÀìDK@Gz `]ŸŸÊÚX[åí–dYÆƱe ÓþyJ’d9³- QçxÉÉŸDlP„ Ti ˜ÿOhV´ƒW7páÂ[ÓuEª'ðz½xùå—Q^®+çn«[¯(Š¼(*Æ\î0ûy¤Põkžç×ê9±ø¬YŸÊë––õ3}óÂ*X%€ˆÄ³Äãñ0«ö°ÎåE$ÒÞÞŽ¿þõ¯tÀf·^Q‡ØªRÎC=„ŽŽŽPHµâY èu&Ê_¥þ‚ÑyêÃè}Öµ¬ôÅj7¿zÌ<ßŨžïe¥-Òscý>ïwinnF}}=}ôQµƒÚ$QÝfkTÀ²X!At«? ±–.\¸€¿þõ¯º|„°Ù­WÅ ÿæ{Ï=÷`Íš5غukȵØî‡:Üû‰ôôXJ•®é‡fRQQ×^{-æn½ýT¯‚r¢ºîºëðÅ/~²,ãsŸû^xádggÛqÉ$.q444 ¡¡S¦LÃá`¥…M鮜St !Ç–Íl³ Ξ=Òö[=@T‡,Ëرcþö·¿±&ÿ löéÁüfÏžo|ãšqÝxã“b—X‹—,Ë8xð œN'Æ:‡áR–¬Hÿÿ¢¢"fÂN#QŸnëîîfæñV$¿ß 6`ß>faÛÝzEQü7“P„PVV†'Ÿ|‡Có#çääàšk®aåH" ø|>ôôô ¬¬L7ÿ6Q¨àÿÚÕ ššš°î½v‰ùjôôôàµ×^Cuu5ý–_–åGöíÛÇUa†¢(ÞàÇê¶aÆᩧž2ÌgP™*Á¥%IDATTT„ÆÆF\¸À”7‰Ë]]]ðx<¡$:4b-ØæÔÑÑÁr°ÑÁnå_ss3^zé%]î¿ß³gÏ:[[[¿*B•,ˆUk¬@Åå¦z !33O=õrss ïÑäÉ“ÑÜÜŒÞÞ^;†’Ä †×ë eÁVWÞ¦3EÑF ñ_Yý#U­°š@jjjðÚk¯éö×>Ÿuuuðz½HII™éóùÞu8¯Ë²|¿,ËÜ>Õ4DQœ‰`¹êP<—Ë…'Ÿ|#FŒ͈ä¦L™‚ýû÷G:„$.´··£§§'”JÏ ôÂ%€© "srr,›ü{e¤+{¤Ÿ;|ø06nܨ“:|>Z[[áp8’’ºÉ‡ã¯×;A„Õ²,[f+QG!Xª:”°P|ë[ߤI“¸"AÀ!CPRR’Ü \†ðxŒ&”ÏçÆ pì˜>`O¥”UŸžü*&ÂA¾*Ëò_ÂEÅÿ/±jÕ*¬Zµ*¢œ‡'NÄáÖ*€'1ÑÛÛ‹ÆÆF°â> …8ÔMà(p•`ÅÈáp ¾ž]¿ÐÎRÝ@P[úÆo ¶VûýAÅ.øý~fºrurÓþà à9Aœ²,ÿÉèšýñéσ*x9wî\¬]»6ªRe999())a¹)'q @–e466¢­­ÍPÃ---ô·Q’$o¸Ï…%€~÷Õ©ê6õ€×pþüùˆyZ‘ðÆoèj:NäææÂápÀï÷Ãçóé&?5ñé®'B¥,Ë\úÿø¼ºaÒ¤Ixä‘G“/ù5 MMMÉ­À%†îînÔÖÖ†$ ²1 À'hj:”Y´3œ€¡¡äï ©ªªÂ† àõj‰/%%Ç­üôêÏýYà–ey½ WÑŠAQ¿ à1u[qq1¾ýíoÃétÚ’ÎLŒ?ž©˜Ä D @}}=š››#^õÕˆÄð€¡øÏkûçqú± ú:ûöíÃöíÛud‘2Y*+¿°‚Rô» “eùMAæË²Ü ¢(ÞàWêssssñ½ï}/T¢<°>—‘‘âââ¤U`£££ç΃Ïç³¼×7B,%Ç žH@€U<#êH/e‚lÙ²Ÿ~ª/¼RTT„‘#G"„V~BˆFü7ý âfçÜ)Šâ|/BS‘ššŠï|ç;2dˆí¤°---I¡Aˆ@ €sçΡ©©É–U_ †À¥0²,ðf‚Š¿ªª*À†hÑ×ׇ7âìYmÌŽ ˜0aB¨Ž€BôêÏ)úk ÿïç Þ+--ýúëÁA=ã>ŠQ£FÙníPc̘1¨¨¨°¥¯$mmm¨®®->€=s š@ ˆ¢X äòGÁ°aø€¼^/3㯬L¶¶6¼ýöÛ´é.— ³gÏÆСC5“ßhõgMüp‘‡‚  ¸¸ø¸Õ×¾ÿþû1mÚ´¨~áššŠ¢¢"ÔÕÕÙÒ_±ƒßïÇ™3gÐÐÐ`ûª¯F¬¶šÕ¿°°P³g1sr8¨¬¬ŒI~¿ÚÚZlܸQ'gdd`þüùÈÊÊ ­Ö~¿_#‰þô8ÌÂŽKJJàr¹4“ÿ–[nÁüùó¤~ (jkkK†'0š››qòäI[÷úFˆÕ@§äuêííµ}Ÿ**++±eËÝD+((Àµ×^‹””¸®¬þ,Ñß¾?ÔGnnn(m—‚… ⦛nðú#GŽd+I"Îðù|8qâDD«~¤Ï€HEEAïÖp&?§Ó‰£GÚ¾Þ³g3†ܸqX¸p¡fo¥^±Y«?-ú­újq»Ýp»5 ?¦OŸŽµk×Ú^¿¯Ün7 “[Bcc#Ž;³½> >ŸNgç`\oOC`EòZ:;;áóù˜ýFr3|>¶nÝŠªª*Ý{W_}5DQÔMfz³Dz\f“_–eWZZŠ‡z_µâh¢›››Ü $<*++Q[[Ó½> MMMô³RÇ›ÐÆLÐÔLOO×ÔT@O&§Ó‰'Npï…ÃMŠžž¼û@B–/_Ž‰'†Vt4íýéóYÄá÷ûáñxt“???<òÜn·åŸhÀº_%%%8vì˜NáÊ’Ò"ùßêçb}­húW¿ŽöZ‡eeehooÇ‘#Gd¯ÏB¤ûÀœ"rjnnæJö¡ÀlR´´´`óæͺ;==·Ür JJJ˜“šÞÿÓ“_}m£½¾rôööꤙŒŒ <òÈ#ÈÌÌ´ÅÖ­¾ ªª §OŸ¶=¦"‰ðØ·orssáv»d¯Ïê#Òý?`”ý¿ÖCër¹˜bºUB0aèÖ­[un½¸ýöÛ‘——Bˆ!Ðâ¿™¹5ñý~?zzztäãr¹ðµ¯} ¡Éo—SS$¨««CeeeT×O"rȲ ÇcI´‘ºHjЪ®®ÎÅß‘#G°wï^Ý ;v,V¯^ŒŒ ¸\®8NCPÄ2–ÂÏhÕ÷ù|¸xñ¢f ‚ à¾ûîØ1cl­dd„p÷­³³3&œˆ·äeû€XTTd*þ»\.ÃT_VöÈ{öìa®h³gÏÆÒ¥K‘’’—Ë:!­>ÃÕ_¹†Ñª¯ÖÜž?^·•¹í¶Û0cÆŒ˜(üX0»o^¯û÷ï×1%%E·5K¸q¶ÏĘh}Ô@*üXˆÅ@SpÈ!Û7‹Îœ9ci?LÞëõbûöíº@‡Ãk¯½³gφÛí†Ëå ™ãÔR‹Ô«?~òBàñxPUU¥ »]¼x1-Z4  ?!X2ýðáú²åYYY(++Ó(ÔÔ¿•ÑëD8w°ŒÉëõâw¿ûæ¾›)þbmb³ë|6‰N§e[´úKuvvbëÖ­hkkÓœãv»qà 7 ¬¬ „Bt –”ñ(“šžü,‘_! eqüøqݾÖ¬Y:GŸHa‡¾àÔ©SºÝív£´´4t­ë0{M÷sÕ¯eü¬1Ñy&€øK1'3 %à'ÒÉqñâElß¾]·âfeeá†n@aaahò«…P&0ðYد2^Öª¯žøŠqüøq8UVV†»îº+$=°`eRG»"466âôéÓš6‡ÃÑ£G3ÄÁ8ñ"ÿ@Œ‰Q?2.¦?5ba䶂ÀJF¨;…3gÎà“O>aºõ.]º4”ÁG=QY$àv»A Mveõ 1*{|uŸÊÿ§N™3g4ã>|8¾ô¥/ÁétšNÜðú‚©ÎXQ€#FŒ%i¹T'^¢Œ‰&€xM~å™ëéé¡·‚=’$éãï  #£@‚ ø9vì˜á0›ååå8zô¨®}Ô¨Q˜7oÒÒÒB× '¬šÔ’€RfK1Òº:X‘jjjtù²³³ñÀVï «¢~8ñù|(//×)ýòóó‘““êãRxÑŒÉÎñÓÛTõB477ÓM–²Å°$Íê?lØ0S÷Yú†˜A‚)¹öîÝ«‹á€ &`Ú´i³}Є "(“5„&¾A²O‚€úúzìÚµK3Ž””|ùË_FNNNÄ?¬ÝDEE…ÎÝ733SS •¾~¼'^¢ŒÉÎñÓ:;%€Hž™hÄ€Mº" j(Çét¢¼¼ÜÒ {{{±sçNk)UpF¶#Rp:HII ¹íˆºŸ––¼÷Þ{šUÕétâž{îÁðáÃc’ÑǪd·J¬¼#FŒ0\åÔHÔÉ”èçò(ã­ŒÆpH4(ðz½:3”ÚÛÛ±k×.Ýg\.&OžŒüü|®~eœr¨5û}}}¡øŘ>tvvâ­·ÞÒ)o½õV”••E•Æ;Üø­ ©© çÎÓ´9Œ9Rm6˜&S¢Œ)’s­èb[`ë€Èr"„àÀÜ^õõõØ»w¯Î§>55eee¡ä™á£H=eÏïõz!Ër(õ—r¨?'Ë2úúú°~ýz›/[¶ 3gÎŒ¹ÂÖ}ëééÁ©S§tíEEEÌÌÌ@ü'H¢ŽÉŽs}>Ÿn‹µ0Üó ÐKS ªg—žžR0Ÿ‰ß]]]†ùééWWW3· ééé1b\.÷„72å¥ê’e9D ^¯WGëÖ­CCCƒf<¢(bÑ¢ER»€úþøý~TVVê”~C† Avv¶æs‰2AyLvœËòˆ„ì\DìÖèR€Ñ7Æår®þjȲŒ#GŽèlÖ@pò@fÊ-õ„7òÚ£=ÿ»?ð™€Bj"xë­·P]]­Ï„ °råʈ3ú(ß×NœŸ'D›7oÆ¡C‡4ã)..Æš5kBÒƒ;&t¤RÁùóçuÖ—Ëe¨c‚\êd`´ÿˆ½¾êëÙM @aa¡æM¿ß²Ý}éžžH’Ä̬˜íxWz³T^ÊÔæ>5ÐRÀîÝ»±mÛ6Íxòòòp×]wYªÞceRGò`´¶¶êܪA@qq±F㜈Ä ‰0¦hÏ¥·ÑnûèëEÛ ?pšúMzŹxñ¢©v¼µµû÷ï‡ÇãѼ§LH¥._¸•>\í>à³É¯Dü©ßSúW¤€#GŽà­·ÞÒŒ)==_øžžnéæÇR ØÛÛ«óF‚‰ê`¬D ‰>¦hÏM/@t¾ŒVI’øMsÐJÌ@ebÕÖÖêÌQjÔÖÖ¢¼¼\G===GµRŽŽÆcM|³É¯œÏ"¿ßšš¼úê«š‰KÁwÜÜÜÜ„ð‚Û¦êêjÝxòòò•ÒËʉ—ˆcŠäÜX:E‚hW@K:  †“¥ªªŠ™ ¨££¡ÉOO|eÒ‡[õÍBzÍ2555áÅ_Ô°¤ X½zu¨jÒ ì jjjt–•´´4 :Ôôº¼/Q&i"Ž‰÷Ü&€pÏU´>'œ>}ÝÝݺ›PQQÁ –ëëë…ööv¤¦¦†&9kâÓ“Þ,o?=ù•mKBèêêÂßþö7ÝvÙ²e?~¼í ?x‰¥¡¡A÷pBtn¾—óŠï1tuuiÞdá°óY‹Ö„!å *\=x¯×‹O?ý” о\WW÷ûÔÔÔZ¼çøfÙ{i‰A9_=¶×_]ç>{õÕWcæÌ™1Qø±Àócwttè¬%‚  °°ÐTéw9¯ØñSGG‡î÷¼d¶¢(Aèt:QPP h¦'Lww7>ýôSVåŸ:«÷ïß¿×áp¬òù|÷¨E|£‰o¥§€µò+«?M²,ãwÞAmm­f`“'O¶”ÑGé+P¾[__³ÔwAA¦I¢L<+ç^jcJõ3iç@èt:áóùtÁ---8zô(+õ÷!7I’t¶ ¿ðûý_ðù|–r/ÜäGtl¿rþ®]»túˆ‘#Gâºë®‹Ip™ÓP ÀùóçucÊÉÉAFF†ég/§‰7câ9—ÞæÚéBc瀹ÿ§WÿÚÚZ£rß|A’¤ñ_–åONçó~¿ÿKáV}3‘ЧùV+ÿÔEA***t9†ŠU«VA„„ ð‚÷’6—¦¦¦"//O×÷`•ã˜xÎ¥=Z3220nÜ8¤¤¤Àëõ¢½½]£Ç‰•ô¨†VôõõiDŒêêjXÝÿðV)¢@ ðï>ŸïN§Ó™f´âå´+¿Ùêïp8pöìY8p@óùÌÌLÜrË-as¶´°¹¹Y§P"„„¶]êqÅ{’Æò:ƒeL§OŸF}}½æ½¬¬¬Púx‡Ãììl :n·~¿ÝÝÝÌØ;a Ež½Ê"%%…;y kLJÜJJJ ÒÓÓCDâõzu:!3Ø!þAÐìÿ³³³QSSÃtKð€[%IÒå!2ƒ,˯:Ži‚ ü¯ØO‹þôê§NÒÝ´9sæ`ܸq1Sø)ã´Š‹/êˆ*%%¹¹¹ 9Abun"ŽI ǃC‡áСCºgÈápÖȈÊÛápÀív#555dÖV<ú|v˜ hžæsçα:€çü¿öÎ.¶­¤Šãÿq®?×NÜÆVšÄŽ“´qì(J4Q·I#Zª ØeßøZ!ZxàeY, â…¨Újµ‚*Pl•jUI‹ZÔ”–¦ÙB«ÒVM ]m’u…òѤI_^›ë{¯û:½?)J2×wf’;sιgfÎù c,{1%sþÆÖÖVò’ʵä÷t¦B$þA ª»»[S}¬¬¬(úZSS—Ë¥‰A¯ ‚p8Œ{÷îáÑ£GŠ‰ÄŸWsssÊ¡¬RèÑh„ÅbIú…!BÒZ,Æ )‰øT&?ð=ÆØói Y çœò…­­­k„^5ͯ@mÑòò²â½Ìëõbpp°¨f.’>`ÙØØP=Fêr¹N?-OB?«Å>mll`ffPÌ÷¼-€[®¯x…1öv>•ËᜯB>‰DþÀ-×üéL©ößÜÜThÓ={öàðáÃÉûjªj-D£QÅ;$_C&„¤V‘~—ÿ,ý;Ô®ËïW»W­ŽíÚÉTO¦þ–«lÿ/ÑhkkkX]]Mùzúôé¶ Ãn·Ãåreåô+5‰`°^¯çÏŸ—_VÆÙÏ1ögJé¼ @irÀ‹Œ±¿ç×]u8çÿ!„¼‰DÞåœ÷|T–Ñô—¾÷Ëͳ]»vaddD5{O)Ìý\4ÜÒÒ’jPG»˜L&8NØl¶ŠïùWCÖ@^yâ`ŒýšRzÀg¬ ¾Ì÷;ÆXz»¨8糄AÎù(çü¥L¦Bû'¼ÿR, FFF’k­ÙP.« ‡P§z „Àjµ¢¶¶V5ͺVØÚÚ’Ÿ!Üɧ®¤]Û0UXײ‡s¾Jy9‰üsþÝX,FÒiÿDþ¿”Ž Ž=Šºººœ4}¹œ€v»=§¬I:•CX­Ö¤^«?ÁãÇå ïc,¯M}±áñÙø!äv,;‹Åä“ßl6§„&â“mxx EsúÃ2¨©©Aww7ZZZ ®\¹¢Y§ü$N¥òÿôrÒÔrÒìÒZçÖ-…Ûî=µÏeCå=8çoBþ$ŠâwDQü†(ŠQa2™àv»y``MMM…ñVéCÞ÷îÞ½ÝÝÝ)‚JE:tj›©tʈ 0›ÍX[[Ãøø¸b;zµ!Ïe‰ÌŽüŒhBç| Àk„ã¢(þ(‹}¾¥¥…ȷȃAttt”Åá¤,ƒ@ ©í¨Z#F£Q±Í7¥ú;žwb±¬V+Ž;† .Tº;y³¼¼¬–">oG½f@ÎùcJé—¢Ñh« “^óù|èéé)ÈìÏÕROÈúúz„B¡”ÌÁÕâìÓ…KÜ»?22‚K—.Uº+y155%ÿ¸–o}šñ ùäw»Ý ”<ˆó¹ß`0`ÿþýhmmM»©“- £Ñˆ#GŽ(òDT“““ò¢3Œ±¼5æ¥ôu_“–9 pÎõ%; P áÒß߯¶¾®Þyç¹BÚp¦Ðz+.(¥ÍÆì’–÷õõ%ËæKº{ëêê’ïö‰ ÅåÐö;EkîTz{{qûöíJwCÁÜÜœÚÒ囌1EFž\©¨ ”:Ÿü{¥åÝÝÝŠ>ÅÀï÷£­­í¹ÔöºðÉŽP(¤ˆ,]I8ç8~ü¸|¬.ø~1ꯘ ”š7ûS2û|>´µµå<ùÓ™û6› Á`N§3%3±Nù¨6ápÿþýí?X&&&Ô6þü€1¦ˆ’”RàWŽHË=@^F~Ïçƒßï!$om_mWGI>ÏPEtvvbff¦=Êž'OžàÌÅkþ¿œ(V•²~ à³Ò§Ó‰žžEP\°X,èééÓéD4­j/¾.|*K$ßï¯Øa®X,†'N`}}]~é›ùDÿMGÙ¥ôU¯IËl6úúú}²]ÛoiiA{{{ò~yšmÜÐ…OQáõzñþûyÛÉÎ9Nž<‰;wGüÏ0ÆŠz¡¬€Rú"€ŸKËL&úûû!BN|, ‚Á êëë³¾¡lí£µgôìÙ3477«&q-£££¸zõª¼ø=_-v[e”Òü@òüeMM z{{a6›³vÌíÝ»Im_Hr…b¢µ«“éB¸y<„Ãá’·îÜ9LLLȋȇçË>µP–”EPJ;ü€5QFA(‚Ýnßvò˜L¦¤¶D"šÝš[)táSzVWWÑØبȖ]LÆÇÇqöìYyqÀËŒ±Ç¥h³ä€RÚ`@J:•}ûö¡¾¾>£æ÷x<èììL&JPqˆèT˜çIø,//£¡¡A5Òs!¬¯¯ãôéÓ˜šRÈ÷*cì/EmPBI¥´ñÔá~iykk+Ün·êäÁ`.— ›››ÓöÏÓÀ®fÊýœ–––àp8Š–ùwvv§NRÍ àÛŒ±·ŠÒPJ&(¥5~`@Zîv»áõzÎápÀï÷Ãjµ‚sžS&Õˆ.€´ËââbNQžÔàœãâÅ‹Ss`G|™1ö›‚É‚RZ'|JZàt:ÑÞÞž¢ùM&<šššh÷èíNB.…#Š"æææÐÙÙ™Ó}ÑhÓÓÓ¸|ùrºåÅEÄóo–%Z )Å` ”~@Êzemm-‚Á A!ñüê^¯6›­(mꃺ:ØiÏi~~@@hTþµ¾¾Žëׯcrr+++éªû7€Of›y»”ÊxEú‹ÙlFWWÌf3S2¬Vón=);m`ïDJñŒq÷î]„B)GZÀ9G8Æüü<>|ˆ›7ofÚŽC<3×댱²&“(º@)µø/$K~]]]EÕö:©è§òlllÀb±`aasssXXXP¤®OÃ_|1¦ˆõUJõ p@ èëèì>ð-£Œ±ŠIðR½¼àg%ª[G§Z\F|GìJ±³/WþñY@üù¯âIEND®B`‚choreonoid-1.1.0+dfsg/src/Choreonoid/icon/choreonoid32.png000066400000000000000000000044701207742442300234070ustar00rootroot00000000000000‰PNG  IHDR szzôsBIT|dˆ pHYs : :ðd’JtEXtSoftwarewww.inkscape.org›î<µIDATX…•Wkl×þîÇ>¼Ë8%^¿ÖìŒ1hÁ†e×Pé"ÅY"þA ©"Q«$¥­¢–(J¥¨•J©ýÓ¨ùÒVªTU©P')X‰®d{׳?–{±ÁëųcÏîìÎí°S¯M >ÒѽsîÌ|çžïž{Ï%Œ1øàT*E&''¢(BEØl¶§î†x<I’àr¹ðæ›ož¢OD ªê?ýôÓ¼ÇãA[[dYÇqOó©%‹‹‹ˆD"Èçó›Í†ÆÆÆçùǽ¼oß¾Îl6{­¯¯/A)}»ººúõ .ð€ (//Ïó‚Ë奄Ƕ”Rp‡ÅÅEØívH’dá¸Ýîº5BȱcǺܸqãß[·n}EQØl6twwãÀžçáv»Q[[kñûÿ”ã8ôôôt]G*•‚®ël@`óöíÛ©Ýn¯N¥R¯lÚ´ ---ظq#zzz iš~§Ó‰L&ƒòòò'R@aH§Óà8eeePUõÞãÖÀ®¶¶6NräȆ¿ßp8Œ^x—/_F6›…®ëp»ÝH§ÓO/ÎzµÌÎÎŽ¯‰€,ËÏ<ó En·;wîÄðð0jkkát:166†––PJ­™]»vÍâºÈ±¿Òîõz-Æâñøм^ï÷)¥p80M­­­¸xñ"Ξ= Ã0 ë:"‘¶mÛÆ&&&¬T³ÛíOl9ŽC.—C&“aÙl6^BA0tVUUy8ŽƒÃá°ì‡Æää$z{{1::Zèïï^\\D,ƒišOEAQŠ‹7•J$VG à÷ûMJ)-ækQŽ?Ž'N<ˆÅbv»=}îܹ¯xžwpŽãÀóß/Ïœ9sB–åiEQî•D@Q”žçñðáC pnn†a ¬¬ pïÞ½1Æû裂~uêÔ)IQ”&Ó4£„cŒ±ý«À¸iš^BÈœ Ž761Æ¢ Ër€ÿQ@¡o½õV¥333°Û혅ÍfCkk+²Ù,âñ8‹ÅbŸuww{E©, QBHR0Mó¥ôf¡PØóùçŸßR%\(Þ!„tBîttt,ž?~™1ìîî^T¥®ÄP(´¥¾¾”R¤R)†¯×‹ªª*¨ª Bóªªþkóæ͆a>÷Üs7ÇÇÇí…Bá7ÍÍÍÙ‘‘‘ßB~àgŒ±oÙ²åÖÈÈÈUÆX SåuÆØ !„1ö×\.wc ¡PèGï½÷ÞDz,Óþþ~ÔÖÖÂív[ëàÌ™3K_ýõ~ÆX?€]ìM¸õIr```”ÇsÐårQŽã (Jñ°€aèêêJÆb±Ý‰C‡ý9¿QÜfW‚ Xíê±ÕÏš¦±/¾øÂ`ç ²²r_1ýæççA¦iøä“O088˜™žž. ‡ÃÇß}÷ÝŸ455qCCCÐu‚ XZ¬ŽEQ,±¯VUUQQQ·Û¾µµµ¬½½½BÄb1 Ü¿ׯ_GSSLÓl²Ûí^}õÕºššNÓ4ø|>Œ­+Þš¦affN§•••Èd2)à9ŽÛÙÐÐ`ªªJÓé4âñ8^{í5$ ¤ÓiìÙ³Gnnn¶¤¦iH§Ó°ÙlVѱRW&”R¤Ói†Ç§Ó xøðá ð‚ éT{{{rïÞ½lii‰¸\.lÛ¶ ’$1çé‹ Æ!ˆÇãÖiZ”ÞÞÞ«cÀŠÃÂáðvìØÑYSSCdY†išÈf³˜››+ùñzÄív—8088Xøæ›o~Q|¶nÇÁ`°ÞçóMøýþüÓÜtÖãL¡P`„ܾ}›E£Ñ×{{{ÿ^û/8øîy)´hIEND®B`‚choreonoid-1.1.0+dfsg/src/Choreonoid/icon/choreonoid48.png000066400000000000000000000074361207742442300234230ustar00rootroot00000000000000‰PNG  IHDR00Wù‡sBIT|dˆ pHYs × ×B(›xtEXtSoftwarewww.inkscape.org›î<›IDAThÅZ{L[Wšÿk_Ûà'/ŠI Äh|Ò4‰h”NÓé$õ¥ÎŽvšv5[5ŠR­´­:êd«Ý­ªÝUÕét3êf3£ª+”´UwgºÓDm6ÂMXš `Þ6?0Øƾ÷žý#¾·66äÕÑ~ÒçñÝëï÷½ÎwÏPJq§ÄqÜ_»\®_³, µZ †a”¦R©Öߊg­µ™™Œ¡¿¿$‘Hüçù~r—Ç>û쳪ÑÑQD£Q¨Õj¨ÕjÈ VßO¾±ÏçC(‚Éd‚$I8xð`pzzúA接ÀóüÏó¿E[·n…ÇãÙl¾›WÝù|>LNN‚0›ÍØ¿1€¿¸+izûرc4•JÁh4¢¹¹ (((ø^„–ijj Xé).— Ã<©¾›—BîÐÇöîÝ[o³ÙF£.— ‘H###H&“«ú5!*•*§ŸÉŸJ¥Fa4sd°X, „”¯iÖÖÖugÌívïN ®Óh4ÿTYY9T__ÿ˲õŸ}öR©”ÒDQ„Á`ÀÂÂ!(((€F£V«…V«…F£Éòs•J¥$‚L¢”"Âëõ"•JåȦV«QTTĬiA¶=üðÃU¿onnÞ¬×ë]UUµ­¼¼EEEÐjµèïï‡ßïGqqqÖ³ÕÕÕðûý(++ËÒìÊþZk à   ())$“ID£QÄb1¤R©È­b µ¾¾žîÞ½»°°°ð´Ýnßæt:QSS—Ë…††lܸ_|ñE–R©, !Â-~bm2X\\D(B$A<‡(Š ”"‰ ¯ Àl6?ÔØØHŒF#öìÙSb±XPQQ§Ó‰ 6 ¡¡mmmG(Ê ¬V+æææî @(‚J¥Ê™Àøª8Ž+¬ªªrY­V7]bÇŽˆD"°Ùl¨®®FCCjjjÐÑÑ“'OæX¡¸¸sss9äN(ƒeÙœy¿ßOŒ¯ÍëׯgäÀ€-[¶`vv“““Ø´iìv;¦¦¦PWW‡S§N!B§Ó€"´ÉdÂÈÈ •<.g#B!J_ÎH™ësss¨¬¬Ì.àVZkjj”L"g‚gžy‡†¼¶´´„……Øívœ>}Û·oÏ ×ë188˜´röÉ7^¹&ï-KKKYÂ¥]smuuuJ&¨ÕjìÛ·|ðžxâ Àìì, ¯¯­­­`YV000Š†Y–½ã¦Ñh`4³@¤sbÕ°Z­[F#!Š[ÈTTT„çŸÇÇÕ«W1<<Œ‰‰ ƒAtww+A<44”7‡ß)QJ¡Õj!Ç#ÌÍÍI¦òhii)¯ªª²É¾¨×ësxêëë±k×.ð<ÁÁAŒŒŒ`ddäB__ßx<ÇØØ"‘È= ŸI²; ‚€`08Ïó¼×…(¥­ëׯWÊ`0ä}ác=†ññq;v,211ñ’$ýóâââO>ÿüó£Z­VÙŒä6ÓÇåù|ØÊ8X9¿¸¸ˆ@ ð¿°Z ´¬_¿^É]]]X^^†ÃáÈa|å•W033£úcOOà·ÇÈܚɟ¡ÿåªX–m[·n’Î"‘t:Âá0 WòâСCÚ—_~ù§ñüùó <ÏŸÎ÷Þ?åÄ!„©¨¨ð°, B Óéàv»1??Ÿ“ÎÀ`0`ûöíQ›ïU ·Ûíq»ÝÉq\a¾uŽãs»Ý'ZZZòØ´i“ËápÈÚ÷ûý0 `Y[¶lÁððpVfI$H¥R˜™™Ã0—ï!äß!ÿN)­Y…åG„NyBZ«««•]rrrR©ÇÍf3ÚÛÛqõêUˆ¢ˆÙÙYD£Q°,‹oww÷ü÷àM?¿téÒ•UXÊ@’$^Zï¿ÿ~Åÿ}>L&“²èt:a±XÐÛÛ‹ÅÅE7?ùæççóú=Çq¹… €7Z=}å|OOÏqžçSJ¥Ìùööö ”ÚÒ|@ž 6[¬V«ò‘L&sªÁp8œµ¹]»v ¾€]»vý‚Rú BÈ(¥?-..~rçÎ)¥»Oœ8Ù¹sg¥ôˆÕjÝ£’$íùꫯúÚÛÛ /8sòäÉ¿J¯?/IÒßêtº†íÛ·a6›”RáÔ©Ss98Ž+¬«««—Óg0ÌÉ:’$!gÍ‚a˜óPVVö#,€ŸøWÕ”Ò-*Ÿzê)MYYÙR«ÕNAÊ(¥ç¼àa§ÓéP`ž{î¹GJKKG9`³$IxÀ4M×*+-Ðl·Û™Ì^YFƒÁ‹ -WWW_Þ¿¿Öjµ6U*Õß½óÎ;×<˜†a$Q_`¡”þêÝwß0~àÀY[Àf³9(¥ „L@IIÉÏ !Œ$I/¿ÿþûýû÷ï÷¥åöÉ¿½@«ÓéTxzz:Ç7nÜ@"‘PÆ‚ Àëõvwuu‰o¿ýv3!„•$éO¯½öÚuÄf³5SJ———‡4ÍC Šb·ü¼ÍfÓÊ…_YY™!“‡*´Ùl»L¼ñÆýi@Ö´kûW¹ÍÏÏ£¶¶ÀÍ‚Šçy¨T*tvvâêÕ«¯×‹d2y.- ’$uÀ‡~X[ZZj¡”þϾ}û¤#GŽ”§„àÈ‘#ÅeeeVI’3-`‚RZ  SÛååår5—ß%%%[ !H¥RH›¢(¢§§‡<ðàÁ„ÏçÃðð0!çÀjµr”RPJ{ ¢¢bsZ žôzœR †a Óãí’$Rúßé±==ž!IJ,¥”*…˜ÍfÛ IR~Çær¹” 4 B«Õ"™LbxxMMM¨¨¨PÜG>*B2™”¸áBzÌ¥]â”––þ–âðŸÝÝÝ|iié^S*•ê­ôzeÚÚãbWW×(¥»»ºº~#IR¢´´ôÇéõ™¢(¶:¥õûýE^¯n·&“)Ë÷ !8}ú4¾ýöÛÃ}}}³i %„N§S`Q§ÓýÚÚÚ~3:::/IÒq³Øû•Jõ«êêêpð‡„?ÕÔÔŒ§-¸O„—!Í’$°‡Rúc•Jõ©"‡@›7oþû_|ñõ¦¦&˜Ífœ9s±X ÍÍÍÐjµŠÐi°øè£hOOÏë<Ï¿ÿGR, V«Ûìv»ÀH$ ”fi>‹áèÑ£)¯×û,ÏóÇ€ã¸MF£ñc–e‹( oˆ$cä&!½~[k™óÉdryrrr'Ïó—ÕòC?þ¸RÊUh&BðñLJfggw^ºt‰O ït8'_}õÕ¢ëׯùHüVìwÛŸœœÄðð0Þzë­N7477»ÊËË 2Óg<W´###øä“Oàõz…éééJ<ÇqE%%%_½þúëÅåååÐh4¼X•A@(‚Åb‘=bøÎ…Z+++•“¯×›õp__xžGii)(¥e Ã|ªÑhõx{ö,R©yäƒAèõzH’D,Ëñ^xA_YY™Åß}÷åýè¹J¥RD2™„ÙlFz‘Ï„¾³€^¯¨¤¤,Ëbll ÀM“}ýõר¯¯Ç¶mÛH$0== I’055…½{÷ê].WŽ«‰¢ŸÏ‡d2‰´r”¿™|™}¹òÍœz½F£1çög~~þ;íííõ*• 7nÜ@<G,ùsçÐÑÑÎÎN8„Ãa‚€ . ¥¥­­­YÙI¦+W®Àd2¡¤¤$çnLÆ[5•J…/¿üÁ`0¯eB¡Pœçù¨“ɤ»¼¼œY^^ÆÄÄB¡xžGSSœN'X–…Á`@<ÇÐІÁŽ;²ÜFÖÜÔÔ!0™LŠ¹ï´I’†aFs.=2d•-6› ýýýE__Ÿd±X˜ôù»â×®]Coo/ž~úi,//ç¼4"£²²ò®…ÏlËËËyO¥EQD ËКH$000€ñññÿ¸~ýzqmmí®@ ½^Q!Ξ=‹'Ÿ|RñíLí'“Iøý~Øl¶ïEøD"BHÞ»±H$I’¦z½¾½··³³³¿¼xñâ!BÈý>Ÿ¯O§Óé%IÂÜܦ§§±{÷n‚A²‚MÎ f³„{~qqçÎCAA, Ìf3ôz½rÐë÷ût€ziiéƒX,výâÅ‹ÇÒ B~"IÒ‘¥¥¥"“É„G} Ãä Z•JI’H$°¼¼¬d›;m²v›•®|'&+M®~§§§@Ù-Õ<ÏÿãJ¡(¥Ÿêtº‹V«õ›¶¶¶û´Z­´„hµZ466Âf³!•Jåv;”ysÃ0 ’É$z{{×¼k¾|ù2>VV?Eccã›n·û¾Ì2Ún·£¶¶F¹]K¨;!I’ V«ÑÔÔ„‘‘‘¼êîî†F£( ¬_¿Ï?ÿ¼R¡PìÁ¼d#t:§=ÏÉ“'O‚‚êêjlݺÅÅÅKÕÅ5SGGFGG甯^½Û·oÏ°c©§èÅwß}Ñh±X J¥ÕÕÕhhh€ÅbYâ®æ§Ë—/Ãívg}¿jÕ*øú’àt:º\®Ö³gÏ"‹ÉZ­FUUjjj ×뗲ˌtá ÍËSUU%‹6‚Û¶mSú|¾ÚÖÖÖ‹éï!Mjµú!N×`2™ªÞÿ}ÔÕÕ!ÝÃhµZTVV¢»»ýë_¥ºém$o’‰’Ë3ñ(•JØívètº¬ò$Þ) €ßïÿ…Z­þ¡ ÿät:’èܨÑh~a³Ù¾e2™ Óé T*Ñ×ׇK—.¡¦¦F&™ôz=c¨©©B¡ÝÇq)Oº+K'}* ùw,CWWúûû±fÍš¬òˆ¢ܵ¸ÁÛ¶nÝŠ–––çA8ßÚÚzÔ`0œÍÏϯÎÍÍ¥z½jµ@‡BeeeJ”Rèt:„B!X­Ö9Âet¡G’ã8äååáêÕ«ˆÅb)}G£Q0Æ033ÇþEPWWgÐh4«JKKaµZÉþ“çùfJiµÍfC~~>xž‡Ñh„F£(ŠhiiAgg'ÊÊÊæ´ÇƒÃáÀÀÀ¬Vë5ÌÁâH¡PÀh4" A­V#"B§ÓA¯×C§Óa||€B¡¨_¶l™¢¨¨^¯MMM9¡Pè¥R «ÕŠ‚‚äççÃl6C§ÓÉ(ðÁøö·¿±M«ÕŠ®®.ø|>PJ—PüÏhvv³³³ÐjµmAÂ=,Ê BËËËa³Ù+V¬ÀæÍ›¡T*A)…Õj…ÍfƒÃá@ii)JJJ°qãFô÷÷cpp0Å#H(Š°Ùlóºªë¥x<ŽH$¥2ûü&–@ÿbÝ`ceeeŠºÞ~ûí(--ÅÔÔÌf3rssQXXˆ’’””” ¨¨ 8vìâñ8âñøòóó155…P(tC§“ßï‡R©œ7½. P©TP«ÕRî»ï>„B!ŒŒŒ@¯×ƒR »Ý›Í“ÉAÐÙ٠ǃX,6Bòòò–\ T*Õ¼Ÿ333sú …BƒA§Ó9±˜%ÐX^^.ûãL¡ì-·Ü‚ 6`ïÞ½èííE?†‡‡166¿ßƒÁ€Ó§Og4†íííRP’1ðIN©TB¥RÉKQ­VC£Ñ@£Ñ@«ÕÊ_²ú‹ùùùsÆ+­¸&!Ycù{î¹”R477£¯¯ƒƒƒp»ÝC<ÇÅ‹á÷ûSl@ÿ’ÀtÒëõ(((H)“b`!¡¼¼\^Ù „à{ßû¦§§qîÜ9 `hhHarr2zæÌÙ>x½ÞŒ[Õ›A …yyy²½HÀ…¡677×H)•5 '''+¿^¯ÇÓO?‘‘ôööbppÃÃÃñ»Ýî^¼xq6 Âëõ¢««ké$\$iµZD£Q ø~`á#±Æ²²²”µi4ç­àp8ðÄOॗ^‚Çã×ë= …cŒõ ‚°úĉ?Ðjµ²Áâ8.Åx¥ÿ–üùŸ%—y½^ôõõ-ÆXãòåËSÀää$ ³ÖÛ¸q#î¿ÿ~ìÛ·ïLUUUÓñãÇ¥]ÉO:;;-–§UI÷e7û·À;À°>Y8ŽÃ¹sç011•J»Ýžµâ£>Š¾¾¾õgÎœy ÀÓàt:'<¸@ŸŸ+eµuuuŽãV•””¤ì½Ýn7Ö®]‹ÑÑQø|¾¬ Bðì³Ï¢¸¸x ߸)£_Ê €B¡¨/**R¨ÕjY¦§§ÁƒÍfCCCzzzR‚¢tÒëõصk|ý&ŒPYY©Y¿~}Æ ²«c‚AøRCCÃïëëë’ʲ@i,--M™ý‘‘9Ä,..Fuu5®\¹"2É‹Å099)Eb×#ÜbÈl6ÿ‡(Šgb±XÛ"Øÿ1¶À½RÁ|nP@zÜnwŠ¬««¥“1†@ €`0“É„¶¶6B>¹ÙK¥‰Ï‘Eð:ŸãRÁ¼PVV–r6744”â !¸ãŽ;Ð×ׇp8Œ¡¡!ø|>0ÆÀÃåË—Á;s‚-Š”Jå.ÆX“(Š›ÁnKŒÛ#×Ïĵnݺ"½^o·Ùl²ðŒ1LLLHÇÉ2©ÕjÜyçØ·o"‘xž—£E—Ë¿ßßít:Ç3õ³ôÉ'Ÿ¸¸â«««3¨T*I}emÉ!¤Ñáp¤ÌþÄÄ„¼I§H$’ñØéÊ•+ «ÿÖ­[ïaŒ=ÌûíäääQžçŸ „ÜA‰‹¢øÆ©S§I¼¹*•êYõ„bÆX'!¤ùÔ©S¯&÷±qãÆÇ9Ž+à8îß>úè£1©|Ó¦MkEQ¼‹²ÀƒÁ°7étz~ „¬—ÜŸôx<ž¬a°Ûí†B¡È@‹ô›Rú cÌ çù¨ ETÕÀŽ;Ö|À+ÄgtÀ6M_ùÊWj:ôCؾ}{©Åby DQ< àp×]w=@)ý/Æ°À#Ò8c2Ùl@c2Çaxx8ëFhhhhÎÙ¿!¤{ì±"J©ƒçyð</¥ô,¦”¾N)MœŒþô§?åÌfóo(¥…‹å7ÍÍÍ+š››·™Íæ¿£”‚çùï<øàƒy`±X*uÁóü(<òÈ#«(¥oQJ Ïó¿r8–œœœe”Ò}I¼ò˜@bØ-¤éGPáp}}}áP(tŒF£ €RÚ¼wïÞ÷cÌl6ëeLNN~‰RÚ@)…Ùl–ÕÝb±\Hð©-Ë&ÈÍ͵KmZ­V˜L¦yžWSJ½ÑhôG¯¿þzôwÞ‰PJ’úϾA¨¥”æ˜L&Y"‘ Ã᧧§FçœÀöôô@Åómmmày^Hh‰HyAâËÍͽ%GœJé­‰W3Ç]’ø(¥ @>JS€Ùl–ÜZä…^ýå/ JéÎDÙ‡¿þõ¯ãR}žç¥CÈ‹/¾èÍ €F‡Ã‘2ûcccÐëõsNYÃá0Nž<‰¼¼¼9K ³³3ÅÿSJ…Ä׫O>ùä0¼ñÆz³Ù\ „³ ¾z‰ï©§ž’¯vxž·H}0Æb ^)úbŒ±—_~ÙN)­N´w"y<”R OrùcË–-Kñ£££s¬¼ßïÇéÓ§Q\\Œ-[¶ ££ÃÃÃ) ÕJ´Je*•j­4³Î&U%M A€ã¸O¼ŽDÙ X,–hÒái4¹>Ïó‰CÔù@’HÏÈÈ´Z­Ì0>>ŽóçÏ£®®«W¯F4ÅòåË¡Óé099 èêêÇq-pèСe<Ï[@E§ÔŽÅb C©TJD¤¨¥tc‚·mçÎ#`6›í‰SOI‡ Œ1eZýe‰¯)c Sâpe:£££òßðð0zzz°qãF8D"¹~nn.€ÏŽœ¦§§GNg/˜L&iöÁ“5 I+Ü[¶lJu@!¤ñĉ÷‹¢x ÀNJé×õÿQªÏó¼d`ûöíácÇŽõ(%„<ðñÇ¿3;;+2ƾE)­LÔÏ®F°Ùl •J%  Š"Ôj5055…­[·‚R*ï“ÏæF#> BÈÇI‚®H| D£Ñdjƒ:!•åç翉D°À¾$ࢌ±Ÿ­[·®9QÄQJ͉þ¯&µù>‹'¶Õét„1v@€@OVâñøzéD2‚cccP«ÕÜvÛmÐjµ)3ŸLï½÷>Ü àÇR™ÉdúoŽãÆ9Ž»`·Ûåƒz‹Å²GÅ $¨­­ ¸­¿¿ÿ~kc&BH‡(Š‡ËÊÊ’w•"Ïó_`ÒëõïJ…õõõ‡»ºº*8Žû;cìTyyùÛÝÝÝÛä«TªƒÉã%ÉÖ»¾¾þvíÚµkÓ¦M0 ÈÉÉÁ§Ÿ~Š+W®   kÖ¬‘#¾ô™h4Š·Þz .\øˆã¸---úF)@i´Ûí).Ðh4¢¤¤åååòÝ^:¼ùæ›Ø;;;ûÉ÷ ‚ ð€"©‹´ÏLeŸÏGN§ó)H;@)cCZV«jµáp8ã=œÛíÆï~÷;6==ý¬Óéüç´×¿­¨¨xX ªÒS[¤¸"SÚKò¤˜ä¥™^>_Éü~¿û÷ïÿ¾ ÔétþÍUB S:fŒI—sˆ‚ŽŽ8p ‰DvK©§ ‚ð³ÊÊʇþóŸãOúcò±÷ÿŧ(Šp¹\0›ÍØ·os:,E!ÅÅÅ)¨ fù––|øá‡!###…„ÂE„ïÚl¶gž{î9ÀæÍ›ÑÒÒ’’³óyÒìì,‚Á ¬V«t)"çÐ%Ûyý'€$!¢(âü#ÚÚÚ““«Õªå8î_=Ï ‚ðU£Ñø«gžy:Nö•••rjÜçE¢(bll ‘HƒŒ1é^PY• Á¸úúzArÇ¡»»{Ž« ‡Ã8rä‚Á êêê a0 T*Á{Èf³]^¾|ùs{öìQX,–”8Á`0 ??_ÀM'ŸÏ‡ááa¨Tª”M\bRS¡Öd2åäääȳ/…´’>ŸGŽAQQ¶lÙ‚P(¯× •J%§ ©TªüqnÙ²eã«Õ ŸÏ—Ñ“,Åb1twwcrrƒaÎ6=1s–@caa¡l)ÛÛÛSðx<8~ü86oÞŒ 6@Eøý~9%nffããã¸÷Þ{¹šššŒ"Ødp“i¾œiiÎGÇ!''Z­Vžõô]jÆ%Àk,**!ñxÓÓÓr§===8wîî¾ûn466‚çyˆ¢ˆññqˆ¢ŸÏ‡‘‘Ü~ûíø¾5B$„ «« ~¿ÒisºµÎ”ø˜‰'_ú}ôèQƒÁ”[:e\‹ŠŠÀq>ýôSD"ùH»¯¯MMM¨®®¥‡CN<ôz½8þ<ªªªðÅ/~1«ÇByy9´ZmÆÛÞä'Ûð|üRbdú¬'ÿž€´,,,D8–7?çÎÃøø8Ö­[‡œœ0ÆLJå<¿£GB­VcÇŽˆFS¶ßò¬ŸåätvvÂn·Ë6c¡']€…žx<Ž™™™y¤¥‰@² Ðh4B^^ž"‰ ½½¡PH¾.,,D4ÅÌÌ |>ÆÇÇeá<—Ë…G}TÎúÈDñxW¯^Enn®œEz-‚-–_Ê ”@ÏtH›HÞbH¾`Œ5Úl6ttt`rr/^dn·û¢(Þ)eZy½^ÙàMMM¡½½ùË_°{÷nùÌ0}Ö%r¹\Ðh40™L7<ËóñOOOg¼³H¦©©)ˆ¢8æt:euUhÔëõèïïGggg(>ÔÓÓóqnnîå©©)«^¯‡J¥‚(Šƒðù|hiiÁ®]» V«³=©Ãh4 ›Í¶ä*ŸÎß×׃Á€ŠŠ 0ÆæŒ+ɵ§ü“BÉ»ÅårÁår øjkkë9 „ü@­V¿“r‡áñxÐÛÛ‹/ùË0)Wãé3 …055%ç߈pó=ÓÓÓèîîFGGJKK‡¡ÕjA)…Á`€F£‘÷ †“ÛW2ÆÞs¹\kc±Øî . %u|@¥Rý cì©H$"§¸mÞ¼YNˆÌFÚ*• ~¿Î.n1ÿÿ[è?ápn·‘HZ­………s‚žx<.Ÿf ¸\.HÉÎR¶¶¶îÉ&H4ÝCù ÿV©T¯_¿v»=k £P(P[[ ‡ÃË—/ˆ1“AºQ2ðx77+W®„Ñh”—KEEżí.(eeeøóŸÿ¼(Þ`0ˆK—.…O._L®ð¿m[¹reJ CAMM ìv»,L ¸&$º0Ö­[‡öööùÞ~ûmÌÌÌ4;Δäáyážçw¯Y³FžyžçQ[[ N‡x<~ÝBg£ë£¼¼\Zß©¿¿GŽ™ðdú»¬‚ð½^ÿìªU«FQ[[+FÒs³i±`ƒAäååeœŒÎÎN¼òÊ+ˆÅbßw:ÃéïI¦NAhR«Õ‡Ö¬Y£°Ûí(--Íø„›aÜ2Ñbû ‡Ãàyƒ¡Pmmmhnnž‰D"?r:ofª“MvFE8Foo/z{{¯wìKN !Š"&&&àv»ãNø®ÓéÌš¥ö¿† [ É~ŒIEND®B`‚choreonoid-1.1.0+dfsg/src/Choreonoid/icon/icon.svg000066400000000000000000003447021207742442300220610ustar00rootroot00000000000000 Choreonoid icon image/svg+xml Choreonoid icon Shin'ichiro Nakaoka choreonoid-1.1.0+dfsg/src/Choreonoid/main.cpp000066400000000000000000000033071207742442300211010ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include #include #include #include using namespace std; using namespace cnoid; int main(int argc, char *argv[]) { cnoid::App app(argc, argv); QIcon icon; icon.addFile(":/Icons/icon/choreonoid32.png"); icon.addFile(":/Icons/icon/choreonoid48.png"); app.initialize("Choreonoid", "Choreonoid", icon, getenv("CNOID_PLUGIN_PATH")); app.exec(); return 0; } #ifdef WIN32 #include #include #include using namespace boost; int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow) { vector argv; char appname[] = "application"; argv.push_back(appname); #ifndef UNICODE vector args = program_options::split_winmain(lpCmdLine); for(size_t i=0; i < args.size(); ++i){ char* arg = new char[args[i].size() + 1]; strcpy(arg, args[i].c_str()); argv.push_back(arg); } #else vector wargs = program_options::split_winmain(lpCmdLine); vector buf; vector args(wargs.size()); int codepage = _getmbcp(); for(size_t i=0; i < args.size(); ++i){ const int size = WideCharToMultiByte(codepage, 0, &wargs[i][0], wargs[i].size(), NULL, 0, NULL, NULL); char* arg; if(size > 0){ arg = new char[size + 1]; WideCharToMultiByte(codepage, 0, &wargs[i][0], wargs[i].size(), arg, size + 1, NULL, NULL); } else { arg = new char[1]; arg[0] = '\0'; } argv.push_back(arg); } #endif return main(argv.size(), &argv[0]); } #endif choreonoid-1.1.0+dfsg/src/Collision/000077500000000000000000000000001207742442300173105ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Collision/CMakeLists.txt000066400000000000000000000021131207742442300220450ustar00rootroot00000000000000 # @author Shin'ichiro Nakaoka set(sources ColdetModel.cpp ColdetModelPair.cpp CollisionPairInserter.cpp TriOverlap.cpp SSVTreeCollider.cpp DistFuncs.cpp Opcode/Ice/IceAABB.cpp Opcode/Ice/IceContainer.cpp Opcode/Ice/IceIndexedTriangle.cpp Opcode/Ice/IceMatrix3x3.cpp Opcode/Ice/IceMatrix4x4.cpp Opcode/Ice/IceRevisitedRadix.cpp Opcode/Ice/IceHPoint.cpp Opcode/Ice/IceRandom.cpp Opcode/Ice/IcePoint.cpp Opcode/OPC_AABBTree.cpp Opcode/OPC_BaseModel.cpp Opcode/OPC_Collider.cpp Opcode/OPC_Common.cpp Opcode/OPC_MeshInterface.cpp Opcode/OPC_Model.cpp Opcode/OPC_OptimizedTree.cpp Opcode/OPC_TreeBuilders.cpp Opcode/OPC_TreeCollider.cpp Opcode/OPC_VolumeCollider.cpp Opcode/OPC_RayCollider.cpp Opcode/OPC_SphereCollider.cpp Opcode/OPC_Picking.cpp ) set(headers CollisionData.h ColdetModel.h ColdetModelPair.h CollisionPairInserter.h exportdecl.h ) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/Opcode) set(target CnoidCollision) add_library(${target} SHARED ${sources}) apply_common_setting_for_library(${target} "${headers}") choreonoid-1.1.0+dfsg/src/Collision/ColdetModel.cpp000066400000000000000000000206341207742442300222140ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ColdetModel.h" #include "ColdetModelSharedDataSet.h" #include "Opcode/Opcode.h" #include using namespace std; using namespace cnoid; ColdetModel::ColdetModel() { dataSet = new ColdetModelSharedDataSet(); isValid_ = false; initialize(); } ColdetModel::ColdetModel(const ColdetModel& org) : name_(org.name_), isValid_(org.isValid_) { dataSet = org.dataSet; initialize(); } void ColdetModel::initialize() { dataSet->refCounter++; transform = new IceMaths::Matrix4x4(); transform->Identity(); pTransform = new IceMaths::Matrix4x4(); pTransform->Identity(); } ColdetModelSharedDataSet::ColdetModelSharedDataSet() { refCounter = 0; pType = ColdetModel::SP_MESH; AABBTreeMaxDepth=0; } ColdetModel::~ColdetModel() { if(--dataSet->refCounter <= 0){ delete dataSet; } delete pTransform; delete transform; } void ColdetModel::setNumVertices(int n) { dataSet->vertices.resize(n); } int ColdetModel::getNumVertices() const { return dataSet->vertices.size(); } void ColdetModel::setNumTriangles(int n) { dataSet->triangles.resize(n); } int ColdetModel::getNumTriangles() const { return dataSet->triangles.size(); } void ColdetModel::setVertex(int index, float x, float y, float z) { dataSet->vertices[index].Set(x, y, z); } void ColdetModel::addVertex(float x, float y, float z) { dataSet->vertices.push_back(IceMaths::Point(x, y, z)); } void ColdetModel::getVertex(int index, float& x, float& y, float& z) const { const Point& v = dataSet->vertices[index]; x = v.x; y = v.y; z = v.z; } void ColdetModel::setTriangle(int index, int v1, int v2, int v3) { udword* mVRef = dataSet->triangles[index].mVRef; mVRef[0] = v1; mVRef[1] = v2; mVRef[2] = v3; } void ColdetModel::getTriangle(int index, int& v1, int& v2, int& v3) const { udword* mVRef = dataSet->triangles[index].mVRef; v1=mVRef[0]; v2=mVRef[1]; v3=mVRef[2]; } void ColdetModel::addTriangle(int v1, int v2, int v3) { dataSet->triangles.push_back(IceMaths::IndexedTriangle(v1, v2, v3)); } void ColdetModel::build() { isValid_ = dataSet->build(); /* unsigned int maxDepth = dataSet->getAABBTreeDepth(); for(unsigned int i=0; i data = getBoundingBoxData(i); cout << "depth= " << i << endl; for(vector::iterator it=data.begin(); it!=data.end(); it++){ cout << (*it).x << " " << (*it).y << " " << (*it).z << endl; } } */ } bool ColdetModelSharedDataSet::build() { bool result = false; if(triangles.size() > 0){ Opcode::OPCODECREATE OPCC; iMesh.SetPointers(&triangles[0], &vertices[0]); iMesh.SetNbTriangles(triangles.size()); iMesh.SetNbVertices(vertices.size()); OPCC.mIMesh = &iMesh; OPCC.mNoLeaf = false; OPCC.mQuantized = false; OPCC.mKeepOriginal = false; model.Build(OPCC); AABBTreeMaxDepth = computeDepth(((Opcode::AABBCollisionTree*)model.GetTree())->GetNodes(), 0, -1) + 1; for(int i=0; igetNumofBB(i)) return i; return getAABBTreeDepth(); } int ColdetModel::getAABBTreeDepth(){ return dataSet->getAABBTreeDepth(); } int ColdetModel::getAABBmaxNum(){ return dataSet->getmaxNumofBB(); } static void getBoundingBoxDataSub (const Opcode::AABBCollisionNode* node, unsigned int currentDepth, unsigned int depth, std::vector& out_data){ if(currentDepth == depth || node->IsLeaf() ){ const IceMaths::Point& p = node->mAABB.mCenter; out_data.push_back(Vector3(p.x, p.y, p.z)); const IceMaths::Point& q = node->mAABB.mExtents; out_data.push_back(Vector3(q.x, q.y, q.z)); } currentDepth++; if(currentDepth > depth) return; if(!node->IsLeaf()){ getBoundingBoxDataSub(node->GetPos(), currentDepth, depth, out_data); getBoundingBoxDataSub(node->GetNeg(), currentDepth, depth, out_data); } } void ColdetModel::getBoundingBoxData(const int depth, std::vector& out_data){ const Opcode::AABBCollisionNode* rootNode=((Opcode::AABBCollisionTree*)dataSet->model.GetTree())->GetNodes(); out_data.clear(); getBoundingBoxDataSub(rootNode, 0, depth, out_data); } int ColdetModelSharedDataSet::computeDepth(const Opcode::AABBCollisionNode* node, int currentDepth, int max ) { /* cout << "depth= " << currentDepth << " "; Point p = node->mAABB.mCenter; cout << p.x << " " << p.y << " " << p.z << " "; p = node->mAABB.mExtents; cout << p.x << " " << p.y << " " << p.z << " "; if(node->IsLeaf()) cout << "is Leaf " ; cout << endl; */ if(max < currentDepth){ max = currentDepth; numBBMap.push_back(0); numLeafMap.push_back(0); } numBBMap.at(currentDepth)++; if(!node->IsLeaf()){ currentDepth++; max = computeDepth(node->GetPos(), currentDepth, max); max = computeDepth(node->GetNeg(), currentDepth, max); }else numLeafMap.at(currentDepth)++; return max; } void ColdetModel::setPosition(const Matrix3& R, const Vector3& p) { transform->Set((float)R(0,0), (float)R(1,0), (float)R(2,0), 0.0f, (float)R(0,1), (float)R(1,1), (float)R(2,1), 0.0f, (float)R(0,2), (float)R(1,2), (float)R(2,2), 0.0f, (float)p(0), (float)p(1), (float)p(2), 1.0f); } void ColdetModel::setPosition(const double* R, const double* p) { transform->Set((float)R[0], (float)R[3], (float)R[6], 0.0f, (float)R[1], (float)R[4], (float)R[7], 0.0f, (float)R[2], (float)R[5], (float)R[8], 0.0f, (float)p[0], (float)p[1], (float)p[2], 1.0f); } void ColdetModel::setPrimitiveType(PrimitiveType ptype) { dataSet->pType = ptype; } ColdetModel::PrimitiveType ColdetModel::getPrimitiveType() const { return dataSet->pType; } void ColdetModel::setNumPrimitiveParams(unsigned int nparam) { dataSet->pParams.resize(nparam); } bool ColdetModel::setPrimitiveParam(unsigned int index, float value) { if (index >= dataSet->pParams.size()) return false; dataSet->pParams[index] = value; return true; } bool ColdetModel::getPrimitiveParam(unsigned int index, float& value) const { if (index >= dataSet->pParams.size()) return false; value = dataSet->pParams[index]; return true; } void ColdetModel::setPrimitivePosition(const double* R, const double* p) { pTransform->Set((float)R[0], (float)R[3], (float)R[6], 0.0f, (float)R[1], (float)R[4], (float)R[7], 0.0f, (float)R[2], (float)R[5], (float)R[8], 0.0f, (float)p[0], (float)p[1], (float)p[2], 1.0f); } double ColdetModel::computeDistanceWithRay(const double *point, const double *dir) { Opcode::RayCollider RC; Ray world_ray(Point((float)point[0], (float)point[1], (float)point[2]), Point((float)dir[0], (float)dir[1], (float)dir[2])); Opcode::CollisionFace CF; Opcode::SetupClosestHit(RC, CF); udword Cache; RC.Collide(world_ray, dataSet->model, transform, &Cache); if (CF.mDistance == FLT_MAX){ return 0; }else{ return CF.mDistance; } } bool ColdetModel::checkCollisionWithPointCloud(const std::vector &i_cloud, double i_radius) { Opcode::SphereCollider SC; SC.SetFirstContact(true); Opcode::SphereCache Cache; IceMaths::Point p(0,0,0); IceMaths::Sphere sphere(p, (float)i_radius); IceMaths::Matrix4x4 sphereTrans(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1); for (unsigned int i=0; imodel, &sphereTrans, transform); if (!isOk) std::cerr << "SphereCollider::Collide() failed" << std::endl; if (SC.GetContactStatus()) return true; } return false; } choreonoid-1.1.0+dfsg/src/Collision/ColdetModel.h000066400000000000000000000147231207742442300216630ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_COLDET_MODEL_H_INCLUDED #define CNOID_COLDET_MODEL_H_INCLUDED #include #include #include #include #include "exportdecl.h" namespace IceMaths { class Matrix4x4; } namespace cnoid { class ColdetModelSharedDataSet; class CNOID_EXPORT ColdetModel : public Referenced { public: enum PrimitiveType { SP_MESH, SP_BOX, SP_CYLINDER, SP_CONE, SP_SPHERE, SP_PLANE }; /** * @brief constructor */ ColdetModel(); /** * @brief copy constructor * * Shape information stored in dataSet is shared with org */ ColdetModel(const ColdetModel& org); /** * @brief destructor */ virtual ~ColdetModel(); /** * @brief set name of this model * @param name name of this model */ inline void setName(const std::string& name) { name_ = name; } /** * @brief get name of this model * @return name name of this model */ inline const std::string& name() const { return name_; } /** * @brief set the number of vertices * @param n the number of vertices */ void setNumVertices(int n); /** * @brief get the number of vertices * @return the number of vertices */ int getNumVertices() const; /** * @brief set the number of triangles * @param n the number of triangles */ void setNumTriangles(int n); int getNumTriangles() const; /** * @brief add a vertex * @param index index of the vertex * @param x x position of the vertex * @param y y position of the vertex * @param z z position of the vertex */ void setVertex(int index, float x, float y, float z); /** add a vertex to the end of the vector */ void addVertex(float x, float y, float z); /** * @brief get a vertex * @param index index of the vertex * @param out_x x position of the vertex * @param out_y y position of the vertex * @param out_z z position of the vertex */ void getVertex(int index, float& out_x, float& out_y, float& out_z) const; /** * @brief add a triangle * @param index index of the triangle * @param v1 index of the first vertex * @param v2 index of the second vertex * @param v3 index of the third vertex */ void setTriangle(int index, int v1, int v2, int v3); /** add a triangle to the end of the vector */ void addTriangle(int v1, int v2, int v3); void getTriangle(int index, int& out_v1, int& out_v2, int& out_v3) const; /** * @brief build tree of bounding boxes to accelerate collision check * * This method must be called before doing collision check */ void build(); /** * @brief check if build() is already called or not * @return true if build() is already called, false otherwise */ inline bool isValid() const { return isValid_; } /** * @brief set position and orientation of this model * @param R new orientation * @param p new position */ void setPosition(const Matrix3& R, const Vector3& p); /** * @brief set position and orientation of this model * @param R new orientation (length = 9) * @param p new position (length = 3) */ void setPosition(const double* R, const double* p); /** * @brief set primitive type * @param ptype primitive type */ void setPrimitiveType(PrimitiveType ptype); /** * @brief get primitive type * @return primitive type */ PrimitiveType getPrimitiveType() const; /** * @brief set the number of parameters of primitive * @param nparam the number of parameters of primitive */ void setNumPrimitiveParams(unsigned int nparam); /** * @brief set a parameter of primitive * @param index index of the parameter * @param value value of the parameter * @return true if the parameter is set successfully, false otherwise */ bool setPrimitiveParam(unsigned int index, float value); /** * @brief get a parameter of primitive * @param index index of the parameter * @param value value of the parameter * @return true if the parameter is gotten successfully, false otherwise */ bool getPrimitiveParam(unsigned int index, float &value) const; /** * @brief set position and orientation of primitive * @param R orientation relative to link (length = 9) * @param p position relative to link (length = 3) */ void setPrimitivePosition(const double* R, const double* p); /** * @brief compute distance between a point and this mesh along ray * @param point a point * @param dir direction of ray * @return distance if ray collides with this mesh, FLT_MAX otherwise */ double computeDistanceWithRay(const double *point, const double *dir); /** * @brief check collision between this triangle mesh and a point cloud * @param i_cloud points * @param i_radius radius of spheres assigned to the points * @return true if colliding, false otherwise */ bool checkCollisionWithPointCloud(const std::vector &i_cloud, double i_radius); void getBoundingBoxData(const int depth, std::vector& out_boxes); int getAABBTreeDepth(); int getAABBmaxNum(); int numofBBtoDepth(int minNumofBB); private: /** * @brief common part of constuctors */ void initialize(); ColdetModelSharedDataSet* dataSet; IceMaths::Matrix4x4* transform; IceMaths::Matrix4x4* pTransform; ///< transform of primitive std::string name_; bool isValid_; friend class ColdetModelPair; }; typedef boost::intrusive_ptr ColdetModelPtr; } #endif choreonoid-1.1.0+dfsg/src/Collision/ColdetModelPair.cpp000066400000000000000000000206131207742442300230250ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "ColdetModelPair.h" #include "ColdetModelSharedDataSet.h" #include "CollisionPairInserter.h" #include "Opcode/Opcode.h" #include "SSVTreeCollider.h" using namespace cnoid; ColdetModelPair::ColdetModelPair() { } ColdetModelPair::ColdetModelPair(ColdetModelPtr model0, ColdetModelPtr model1, double tolerance) { models[0] = model0; models[1] = model1; tolerance_ = tolerance; } ColdetModelPair::ColdetModelPair(const ColdetModelPair& org) { models[0] = org.models[0]; models[1] = org.models[1]; tolerance_ = org.tolerance_; } ColdetModelPair::~ColdetModelPair() { } void ColdetModelPair::set(ColdetModelPtr model0, ColdetModelPtr model1) { models[0] = model0; models[1] = model1; } std::vector& ColdetModelPair::detectCollisionsSub(bool detectAllContacts) { collisionPairInserter.clear(); bool detected; if ((models[0]->getPrimitiveType() == ColdetModel::SP_PLANE && models[1]->getPrimitiveType() == ColdetModel::SP_CYLINDER) || (models[1]->getPrimitiveType() == ColdetModel::SP_PLANE && models[0]->getPrimitiveType() == ColdetModel::SP_CYLINDER)){ detected = detectPlaneCylinderCollisions(detectAllContacts); }else{ detected = detectMeshMeshCollisions(detectAllContacts); } if(!detected){ collisionPairInserter.clear(); } return collisionPairInserter.collisions(); } bool ColdetModelPair::detectMeshMeshCollisions(bool detectAllContacts) { bool result = false; if(models[0]->isValid() && models[1]->isValid()){ Opcode::BVTCache colCache; // inverse order because of historical background // this should be fixed.(note that the direction of normal is inversed when the order inversed colCache.Model0 = &models[1]->dataSet->model; colCache.Model1 = &models[0]->dataSet->model; Opcode::AABBTreeCollider collider; collider.setCollisionPairInserter(&collisionPairInserter); if(!detectAllContacts){ collider.SetFirstContact(true); } result = collider.Collide(colCache, models[1]->transform, models[0]->transform); boxTestsCount = collider.GetNbBVBVTests(); triTestsCount = collider.GetNbPrimPrimTests(); } return result; } bool ColdetModelPair::detectPlaneCylinderCollisions(bool detectAllContacts) { ColdetModelPtr plane, cylinder; bool reversed=false; if (models[0]->getPrimitiveType() == ColdetModel::SP_PLANE){ plane = models[0]; }else if(models[0]->getPrimitiveType() == ColdetModel::SP_CYLINDER){ cylinder = models[0]; } if (models[1]->getPrimitiveType() == ColdetModel::SP_PLANE){ plane = models[1]; reversed = true; }else if(models[1]->getPrimitiveType() == ColdetModel::SP_CYLINDER){ cylinder = models[1]; } if (!plane || !cylinder) return false; IceMaths::Matrix4x4 pTrans = (*(plane->pTransform)) * (*(plane->transform)); IceMaths::Matrix4x4 cTrans = (*(cylinder->pTransform)) * (*(cylinder->transform)); float radius, height; // height and radius of cylinder cylinder->getPrimitiveParam(0, radius); cylinder->getPrimitiveParam(1, height); IceMaths::Point pTopLocal(0, height/2, 0), pBottomLocal(0, -height/2, 0); IceMaths::Point pTop, pBottom; // center points of top and bottom discs IceMaths::TransformPoint4x3(pTop, pTopLocal, cTrans); IceMaths::TransformPoint4x3(pBottom, pBottomLocal, cTrans); IceMaths::Point pOnPlane, nLocal(0,0,1), n; IceMaths::TransformPoint3x3(n, nLocal, pTrans); pTrans.GetTrans(pOnPlane); float d = pOnPlane|n; // distance between origin and plane float dTop = (pTop|n) - d; float dBottom = (pBottom|n) - d; if (dTop > radius && dBottom > radius) return false; float theta = asinf((dTop - dBottom)/height); float rcosth = radius*cosf(theta); int contactsCount = 0; if (rcosth >= dTop) contactsCount+=2; if (rcosth >= dBottom) contactsCount+=2; if (contactsCount){ std::vector& cdata = collisionPairInserter.collisions(); cdata.resize(contactsCount); for (int i=0; i < contactsCount; i++){ cdata[i].num_of_i_points = 1; cdata[i].i_point_new[0]=1; cdata[i].i_point_new[1]=0; cdata[i].i_point_new[2]=0; cdata[i].i_point_new[3]=0; if (reversed){ cdata[i].n_vector[0] = -n.x; cdata[i].n_vector[1] = -n.y; cdata[i].n_vector[2] = -n.z; }else{ cdata[i].n_vector[0] = n.x; cdata[i].n_vector[1] = n.y; cdata[i].n_vector[2] = n.z; } } IceMaths::Point vBottomTop = pTop - pBottom; IceMaths::Point v = vBottomTop^n; v.Normalize(); IceMaths::Point w = v^n; w.Normalize(); unsigned int index=0; if (rcosth >= dBottom){ // bottom disc collides float depth = rcosth - dBottom; IceMaths::Point iPoint = pBottom - dBottom * n - dBottom * tanf(theta) * w; float x = dBottom / cosf(theta); IceMaths::Point dv = sqrtf(radius*radius - x*x)*v; cdata[index].i_points[0][0] = iPoint.x + dv.x; cdata[index].i_points[0][1] = iPoint.y + dv.y; cdata[index].i_points[0][2] = iPoint.z + dv.z; cdata[index].depth = depth; index++; cdata[index].i_points[0][0] = iPoint.x - dv.x; cdata[index].i_points[0][1] = iPoint.y - dv.y; cdata[index].i_points[0][2] = iPoint.z - dv.z; cdata[index].depth = depth; index++; } if (rcosth >= dTop){ // top disc collides float depth = rcosth - dTop; IceMaths::Point iPoint = pTop - dTop*n - dTop*tan(theta)*w; float x = dTop / cosf(theta); IceMaths::Point dv = sqrtf(radius*radius - x*x) * v; cdata[index].i_points[0][0] = iPoint.x + dv.x; cdata[index].i_points[0][1] = iPoint.y + dv.y; cdata[index].i_points[0][2] = iPoint.z + dv.z; cdata[index].depth = depth; index++; cdata[index].i_points[0][0] = iPoint.x - dv.x; cdata[index].i_points[0][1] = iPoint.y - dv.y; cdata[index].i_points[0][2] = iPoint.z - dv.z; cdata[index].depth = depth; index++; } return true; } return false; } double ColdetModelPair::computeDistance(double *point0, double *point1) { if(models[0]->isValid() && models[1]->isValid()){ Opcode::BVTCache colCache; colCache.Model0 = &models[1]->dataSet->model; colCache.Model1 = &models[0]->dataSet->model; SSVTreeCollider collider; float d; Point p0, p1; collider.Distance(colCache, d, p0, p1, models[1]->transform, models[0]->transform); point0[0] = p1.x; point0[1] = p1.y; point0[2] = p1.z; point1[0] = p0.x; point1[1] = p0.y; point1[2] = p0.z; return d; } return -1; } double ColdetModelPair::computeDistance(int& triangle0, double* point0, int& triangle1, double* point1) { if(models[0]->isValid() && models[1]->isValid()){ Opcode::BVTCache colCache; colCache.Model0 = &models[1]->dataSet->model; colCache.Model1 = &models[0]->dataSet->model; SSVTreeCollider collider; float d; Point p0, p1; collider.Distance(colCache, d, p0, p1, models[1]->transform, models[0]->transform); point0[0] = p1.x; point0[1] = p1.y; point0[2] = p1.z; point1[0] = p0.x; point1[1] = p0.y; point1[2] = p0.z; triangle1 = colCache.id0; triangle0 = colCache.id1; return d; } return -1; } bool ColdetModelPair::detectIntersection() { if(models[0]->isValid() && models[1]->isValid()){ Opcode::BVTCache colCache; colCache.Model0 = &models[1]->dataSet->model; colCache.Model1 = &models[0]->dataSet->model; SSVTreeCollider collider; return collider.Collide(colCache, tolerance_, models[1]->transform, models[0]->transform); } return false; } choreonoid-1.1.0+dfsg/src/Collision/ColdetModelPair.h000066400000000000000000000043761207742442300225020ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_COLLISION_COLDET_MODEL_PAIR_H_INCLUDED #define CNOID_COLLISION_COLDET_MODEL_PAIR_H_INCLUDED #include "CollisionData.h" #include "ColdetModel.h" #include "CollisionPairInserter.h" #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT ColdetModelPair : public Referenced { public: ColdetModelPair(); ColdetModelPair(ColdetModelPtr model0, ColdetModelPtr model1, double tolerance=0); ColdetModelPair(const ColdetModelPair& org); virtual ~ColdetModelPair(); void set(ColdetModelPtr model0, ColdetModelPtr model1); inline ColdetModel* model(int index) { return models[index].get(); } inline std::vector& detectCollisions() { return detectCollisionsSub(true); } inline std::vector& collisions() { return collisionPairInserter.cdContact; } inline void clearCollisions(){ collisionPairInserter.cdContact.clear(); } inline bool checkCollision() { return !detectCollisionsSub(false).empty(); } double computeDistance(double *point0, double *point1); /** @param out_triangle0, out_triangle1 Indices of the triangle pair that are originally registered by ColdeModel::setTraiangle(). @param out_point0, out_point1 The closest points */ double computeDistance(int& out_triangle0, double* out_point0, int& out_triangle1, double* out_point1); bool detectIntersection(); double tolerance() const { return tolerance_; } void setTolerance(double tolerance){ tolerance_ = tolerance; } private: std::vector& detectCollisionsSub(bool detectAllContacts); bool detectMeshMeshCollisions(bool detectAllContacts); bool detectPlaneCylinderCollisions(bool detectAllContacts); ColdetModelPtr models[2]; double tolerance_; CollisionPairInserter collisionPairInserter; int boxTestsCount; int triTestsCount; }; typedef boost::intrusive_ptr ColdetModelPairPtr; } #endif choreonoid-1.1.0+dfsg/src/Collision/ColdetModelSharedDataSet.h000066400000000000000000000024711207742442300242550ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_COLDET_MODEL_SHARED_DATA_SET_H_INCLUDED #define CNOID_COLDET_MODEL_SHARED_DATA_SET_H_INCLUDED #include "ColdetModel.h" #include "Opcode/Opcode.h" #include using namespace std; using namespace cnoid; namespace cnoid { class ColdetModelSharedDataSet { public: ColdetModelSharedDataSet(); bool build(); // need two instances ? Opcode::Model model; Opcode::MeshInterface iMesh; vector vertices; vector triangles; ColdetModel::PrimitiveType pType; std::vector pParams; inline int getAABBTreeDepth() { return AABBTreeMaxDepth; }; inline int getNumofBB(int depth){ return numBBMap.at(depth); }; inline int getmaxNumofBB(){ if(AABBTreeMaxDepth>0){ return numBBMap.at(AABBTreeMaxDepth-1); } else { return 0; } }; private: int refCounter; int AABBTreeMaxDepth; std::vector numBBMap; std::vector numLeafMap; int computeDepth(const Opcode::AABBCollisionNode* node, int currentDepth, int max ); friend class ColdetModel; }; } #endif choreonoid-1.1.0+dfsg/src/Collision/CollisionData.h000066400000000000000000000012261207742442300222070ustar00rootroot00000000000000 #ifndef CNOID_COLLISION_COLLISION_DATA_H_INCLUDED #define CNOID_COLLISION_COLLISION_DATA_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { // this is for the client class collision_data { public: int id1; int id2; int num_of_i_points; Vector3 i_points[4]; int i_point_new[4]; Vector3 n_vector; double depth; Vector3 n; // normal vector of triangle id1 Vector3 m; // normal vector of triangle id2 int c_type; // c_type=1 for vertex-face contact, c_type=2 for edge-edge contact }; } #endif choreonoid-1.1.0+dfsg/src/Collision/CollisionPairInserter.cpp000066400000000000000000000422361207742442300243060ustar00rootroot00000000000000 #include "CollisionPairInserter.h" #include "Opcode/Opcode.h" #include #include using namespace std; using namespace Opcode; using namespace cnoid; int tri_tri_overlap( const Vector3& P1, const Vector3& P2, const Vector3& P3, const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, collision_data* col_p, CollisionPairInserter* collisionPairInserter); namespace { const bool COLLIDE_DEBUG = false; // ƒRƒ“ƒpƒCƒ‹Žž‚É@-DDEPTH_CHECK‚Æ‚·‚ê‚ÎAdepth’l‚É‚æ‚éÚG“_‘I‘ð‚ª—LŒø‚É‚È‚è‚Ü‚·B@@// #ifdef DEPTH_CHECK const double MAX_DEPTH = 0.1; #endif const int CD_OK = 0; const int CD_ALL_CONTACTS = 1; const int CD_FIRST_CONTACT = 2; const int CD_ERR_COLLIDE_OUT_OF_MEMORY = 2; enum { FV = 1, VF, EE }; } CollisionPairInserter::CollisionPairInserter() { } CollisionPairInserter::~CollisionPairInserter() { } void CollisionPairInserter::copy_tri(col_tri* t1, tri* t2) { t1->p1 = t2->p1; t1->p2 = t2->p2; t1->p3 = t2->p3; } void CollisionPairInserter::copy_tri(col_tri* t1, col_tri* t2) { t1->p1 = t2->p1; t1->p2 = t2->p2; t1->p3 = t2->p3; if(t2->n[0] && t2->n[1] && t2->n[2]){ t1->n = t2->n; } } void CollisionPairInserter::calc_normal_vector(col_tri* t) { if(t->status == 0){ const Vector3 e1 = t->p2 - t->p1; const Vector3 e2 = t->p3 - t->p2; t->n = e1.cross(e2).normalized(); t->status = 1; } } int CollisionPairInserter::is_convex_neighbor(col_tri* t1, col_tri* t2) { const double EPS = 1.0e-12; // a small number calc_normal_vector(t1); // printf("normal vector1 = %f %f %f\n", t1->n[0], t1->n[1], t1->n[2]); // printf("normal vector2 = %f %f %f\n", t2->n[0], t2->n[1], t2->n[2]); const Vector3 vec1 = t2->p1 - t1->p1; const Vector3 vec2 = t2->p2 - t1->p2; const Vector3 vec3 = t2->p3 - t1->p3; // printf("is_convex_neighbor = %f %f %f\n",innerProd(t1->n,vec1),innerProd(t1->n,vec2),innerProd(t1->n,vec3)); if(t1->n.dot(vec1) < EPS && t1->n.dot(vec2) < EPS && t1->n.dot(vec3) < EPS){ return 1; } else { return 0; } } int CollisionPairInserter::identical_ver(const Vector3& v1, const Vector3& v2) { int num = 0; const double EPS = 1.0e-6; // 1 micro meter if(fabs(v1[0]-v2[0]) < EPS) ++num; if(fabs(v1[1]-v2[1]) < EPS) ++num; if(fabs(v1[2]-v2[2]) < EPS) ++num; return (num==3) ? 1 : 0; } int CollisionPairInserter::is_neighboring_triangle(col_tri* t1, col_tri* t2) { int num = identical_ver(t1->p1,t2->p1) + identical_ver(t1->p1,t2->p2) + identical_ver(t1->p1,t2->p3) + identical_ver(t1->p2,t2->p1) + identical_ver(t1->p2,t2->p2) + identical_ver(t1->p2,t2->p3) + identical_ver(t1->p3,t2->p1) + identical_ver(t1->p3,t2->p2) + identical_ver(t1->p3,t2->p3); // printf("is_neighboring_triangle = %d\n", num); return (num==2) ? 1 : 0; } void CollisionPairInserter::get_neighboring_triangles (col_tri* tri_convex_neighbor, col_tri* tri_neighbor, int* start_tri, int* end_tri, int num_tri) { int i, j, m, m_previous; m = *end_tri; for(i = *start_tri; i < *end_tri; ++i){ m_previous = m; for(j = *end_tri; j < num_tri; ++j){ if(tri_neighbor[j].status != 2 && is_neighboring_triangle(&tri_convex_neighbor[i], &tri_neighbor[j]) && is_convex_neighbor(&tri_convex_neighbor[i], &tri_neighbor[j])){ tri_neighbor[j].status = 2; copy_tri(&tri_convex_neighbor[m], &tri_neighbor[j]); ++m; } if(m==m_previous+2) break; } } *start_tri = *end_tri; *end_tri = m; } int CollisionPairInserter::get_triangles_in_convex_neighbor (tri* root, col_tri* tri_convex_neighbor, col_tri* tri_neighbor, int num_tri, int max_num) { int i, start_tri, end_tri; copy_tri(&tri_convex_neighbor[0], root); tri_convex_neighbor[0].status = 0; calc_normal_vector(&tri_convex_neighbor[0]); start_tri = 0; end_tri = 1; for(i=0; i < max_num; ++i){ if(start_tri < end_tri){ get_neighboring_triangles(tri_convex_neighbor, tri_neighbor, &start_tri, &end_tri, num_tri); } } return end_tri; } int CollisionPairInserter::get_triangles_in_convex_neighbor (tri* root, col_tri* tri_convex_neighbor, col_tri* tri_neighbor, int num_tri) { const int MAX_EXPANSION_NUM = 3; return get_triangles_in_convex_neighbor(root, tri_convex_neighbor, tri_neighbor, num_tri, MAX_EXPANSION_NUM); } void CollisionPairInserter::get_triangles_in_neighbor( col_tri* neighbor_tris, int* n, const Opcode::AABBCollisionNode* root, Opcode::MeshInterface* mesh) { if(root->IsLeaf()){ //$B;03Q7A$N%G!<%?$rJQ49$9$k!#(B tri t; Opcode::VertexPointers vp; mesh->GetTriangle(vp, (root->GetPrimitive())); t.p1[0] = (double)vp.Vertex[0]->x; t.p1[1] = (double)vp.Vertex[0]->y; t.p1[2] = (double)vp.Vertex[0]->z; t.p2[0] = (double)vp.Vertex[1]->x; t.p2[1] = (double)vp.Vertex[1]->y; t.p2[2] = (double)vp.Vertex[1]->z; t.p3[0] = (double)vp.Vertex[2]->x; t.p3[1] = (double)vp.Vertex[2]->y; t.p3[2] = (double)vp.Vertex[2]->z; copy_tri(&neighbor_tris[*n], &t); *n += 1; } else { if(root->GetPos()) get_triangles_in_neighbor(neighbor_tris, n, root->GetPos(), mesh); if(root->GetNeg()) get_triangles_in_neighbor(neighbor_tris, n, root->GetNeg(), mesh); } } int CollisionPairInserter::count_num_of_triangles(const Opcode::AABBCollisionNode* root) { int num_p = 0; int num_n = 0; if(root->IsLeaf()) return 1; else{ if(root->GetPos()) num_p = count_num_of_triangles(root->GetPos()); if(root->GetNeg()) num_n = count_num_of_triangles(root->GetNeg()); } return num_p + num_n; } void CollisionPairInserter::examine_normal_vector( const Opcode::AABBCollisionNode* b1, const Opcode::AABBCollisionNode* b2, int ctype, Opcode::MeshInterface* mesh1, Opcode::MeshInterface* mesh2) { static const int NUM_TRI = 10; // number of the neighbors of the neighbors static const int MAX_BACKTRACK_LEVEL = 3; // ascend the box tree by backtrack_level times at most // // get the root node of each neighbor // const Opcode::AABBCollisionNode* root_of_neighbor1 = (Opcode::AABBCollisionNode*)b1; int num_tri1 = 0; int k = 0; while(num_tri1 < NUM_TRI && k < MAX_BACKTRACK_LEVEL){ ++k; for(int i=0; i < k; ++i){ if(root_of_neighbor1 != root_of_neighbor1->GetB()) root_of_neighbor1 = root_of_neighbor1->GetB(); } num_tri1 = count_num_of_triangles(root_of_neighbor1); if(COLLIDE_DEBUG) printf("num of triangles1 = %d\n", num_tri1); } const Opcode::AABBCollisionNode* root_of_neighbor2 = (Opcode::AABBCollisionNode*)b2; int num_tri2 = 0; k = 0; while(num_tri2 < NUM_TRI && k < MAX_BACKTRACK_LEVEL){ ++k; for(int i=0; i < k; ++i){ if(root_of_neighbor2 != root_of_neighbor2->GetB()) root_of_neighbor2 = root_of_neighbor2->GetB(); } num_tri2 = count_num_of_triangles(root_of_neighbor2); if(COLLIDE_DEBUG) printf("num of triangles2 = %d\n", num_tri2); } check_separability(b1, root_of_neighbor1, num_tri1, b2, root_of_neighbor2, num_tri2, ctype, mesh1, mesh2); } void CollisionPairInserter::check_separability( const Opcode::AABBCollisionNode* b1, const Opcode::AABBCollisionNode* root1, int num_tri1, const Opcode::AABBCollisionNode* b2, const Opcode::AABBCollisionNode* root2, int num_tri2, int ctype, Opcode::MeshInterface* mesh1, Opcode::MeshInterface* mesh2) { int contactIndex = cdContact.size() - 1; Vector3 signed_distance; Vector3 signed_distance1 = Vector3::Constant(99999999.0); Vector3 signed_distance2 = Vector3::Constant(-99999999.0); // signed_distance is positive when a vertex is outside b2 find_signed_distance(signed_distance1, b1, root1, num_tri1, contactIndex, ctype,1, mesh1); find_signed_distance(signed_distance2, b2, root2, num_tri2, contactIndex, ctype,2, mesh2); int max = (2 < ctype) ? ctype : 2; for(int i=0; i < max; ++i){ signed_distance[i] = signed_distance1[i] - signed_distance2[i]; if(COLLIDE_DEBUG) printf("signed distance %d = %f\n", i, signed_distance[i]); } switch(ctype){ case FV: if(signed_distance[0] < signed_distance[1]){ cdContact[contactIndex].n_vector = cdContact[contactIndex].m; cdContact[contactIndex].depth = fabs(signed_distance[1]); if(COLLIDE_DEBUG) printf("normal replaced\n"); } else { cdContact[contactIndex].depth = fabs(signed_distance[0]); } break; case VF: if(signed_distance[0] < signed_distance[1]){ cdContact[contactIndex].n_vector = - cdContact[contactIndex].n; cdContact[contactIndex].depth = fabs(signed_distance[1]); if(COLLIDE_DEBUG) printf("normal replaced\n"); } else { cdContact[contactIndex].depth = fabs(signed_distance[0]); } break; case EE: cdContact[contactIndex].num_of_i_points = 1; if(signed_distance[0] < signed_distance[1] && signed_distance[2] <= signed_distance[1]){ cdContact[contactIndex].n_vector = cdContact[contactIndex].m; cdContact[contactIndex].depth = fabs(signed_distance[1]); if(COLLIDE_DEBUG) printf("normal replaced\n"); } else if(signed_distance[0] < signed_distance[2] && signed_distance[1] < signed_distance[2]){ cdContact[contactIndex].n_vector = - cdContact[contactIndex].n; cdContact[contactIndex].depth = fabs(signed_distance[2]); if(COLLIDE_DEBUG) printf("normal replaced\n"); } else { cdContact[contactIndex].depth = fabs(signed_distance[0]); // cout << "depth in InsertCollisionPair.cpp = " << signed_distance[0] << endl; } cdContact[contactIndex].i_points[0] += cdContact[contactIndex].i_points[1]; cdContact[contactIndex].i_points[0] *= 0.5; break; } if(COLLIDE_DEBUG){ printf("final normal = %f %f %f\n", cdContact[contactIndex].n_vector[0], cdContact[contactIndex].n_vector[1], cdContact[contactIndex].n_vector[2]); } if(COLLIDE_DEBUG){ for(int i=0; i < cdContact[contactIndex].num_of_i_points; ++i){ cout << "final depth = " << cdContact[contactIndex].depth << endl; cout << "final i_point = " << cdContact[contactIndex].i_points[i][0] << " " << cdContact[contactIndex].i_points[i][1] << " " << cdContact[contactIndex].i_points[i][2] << endl; } } if(COLLIDE_DEBUG) cout << endl; } void CollisionPairInserter::find_signed_distance( Vector3& signed_distance, col_tri* trp, int nth, int ctype, int obj) { find_signed_distance(signed_distance, trp->p1, nth, ctype, obj); find_signed_distance(signed_distance, trp->p2, nth, ctype, obj); find_signed_distance(signed_distance, trp->p3, nth, ctype, obj); } void CollisionPairInserter::find_signed_distance( Vector3& signed_distance, const Vector3& vert, int nth, int ctype, int obj) { Vector3 vert_w; if(obj==1){ vert_w = CD_s1 * Vector3(CD_Rot1 * vert + CD_Trans1); } else { vert_w = CD_s2 * Vector3(CD_Rot2 * vert + CD_Trans2); } if(COLLIDE_DEBUG) printf("vertex = %f %f %f\n", vert_w[0], vert_w[1], vert_w[2]); // use the first intersecting point to find the distance const Vector3 vec = vert_w - cdContact[nth].i_points[0]; cdContact[nth].n_vector.normalize(); double dis0 = cdContact[nth].n_vector.dot(vec); #if 0 switch(ctype){ case FV: if(dot(cdContact[nth].n_vector, cdContact[nth].n) > 0.0) dis0 = - dis0; break; case VF: if(dot(cdContact[nth].n_vector, cdContact[nth].m) < 0.0) dis0 = - dis0; break; case EE: if(dot(cdContact[nth].n_vector, cdContact[nth].n) > 0.0 || dot(cdContact[nth].n_vector, cdContact[nth].m) < 0.0) dis0 = - dis0; } #endif if(COLLIDE_DEBUG) printf("dis0 = %f\n", dis0); double dis1 = dis0; double dis2 = dis0; switch(ctype){ case FV: dis1 = cdContact[nth].m.dot(vec); if(COLLIDE_DEBUG) printf("dis1 = %f\n", dis1); break; case VF: dis1 = - cdContact[nth].n.dot(vec); if(COLLIDE_DEBUG) printf("dis1 = %f\n", dis1); break; case EE: dis1 = cdContact[nth].m.dot(vec); dis2 = - cdContact[nth].n.dot(vec); if(COLLIDE_DEBUG){ printf("dis1 = %f\n", dis1); printf("dis2 = %f\n", dis2); } } if(COLLIDE_DEBUG) printf("obj = %d\n", obj); if(obj == 1){ if(dis0 < signed_distance[0]) signed_distance[0] = dis0; if(dis1 < signed_distance[1]) signed_distance[1] = dis1; if(ctype==EE) if(dis2 < signed_distance[2]) signed_distance[2] = dis2; } else{ if(signed_distance[0] < dis0) signed_distance[0] = dis0; if(signed_distance[1] < dis1) signed_distance[1] = dis1; if(ctype==EE) if(signed_distance[2] < dis2) signed_distance[2] = dis2; } } void CollisionPairInserter::find_signed_distance( Vector3& signed_distance, const Opcode::AABBCollisionNode* b1, const Opcode::AABBCollisionNode* root, int num_tri, int contactIndex, int ctype, int obj, Opcode::MeshInterface* mesh ) { int num; col_tri* tri_neighbor = new col_tri[num_tri]; col_tri* tri_convex_neighbor = new col_tri[num_tri]; for(int i=0; iGetTriangle(vp, (b1->GetPrimitive())); t.p1[0] = vp.Vertex[0]->x; t.p1[1] = vp.Vertex[0]->y; t.p1[2] = vp.Vertex[0]->z; t.p2[0] = vp.Vertex[1]->x; t.p2[1] = vp.Vertex[1]->y; t.p2[2] = vp.Vertex[1]->z; t.p3[0] = vp.Vertex[2]->x; t.p3[1] = vp.Vertex[2]->y; t.p3[2] = vp.Vertex[2]->z; // get the triangles in the convex neighbor of the root triangle and their normal vectors num = get_triangles_in_convex_neighbor(&t, tri_convex_neighbor, tri_neighbor, num_tri); // if(COLLIDE_DEBUG) printf("num of triangles in convex neighbor = %d\n", num); // note that num = num of convex neighbor + 1 for(int i=0; i namespace Opcode { class AABBCollisionNode; class MeshInterface; } namespace cnoid { class CollisionPairInserter { public: CollisionPairInserter(); ~CollisionPairInserter(); void clear(){ cdContact.clear(); } int detectTriTriOverlap( const Vector3& P1, const Vector3& P2, const Vector3& P3, const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, collision_data* col_p); int apply(const Opcode::AABBCollisionNode* b1, const Opcode::AABBCollisionNode* b2, int id1, int id2, int num_of_i_points, Vector3 i_points[4], Vector3& n_vector, double depth, Vector3& n1, Vector3& m1, int ctype, Opcode::MeshInterface* mesh1, Opcode::MeshInterface* mesh2); inline std::vector& collisions() { return cdContact; } Matrix3 CD_Rot1; Vector3 CD_Trans1; double CD_s1; Matrix3 CD_Rot2; Vector3 CD_Trans2; double CD_s2; std::vector cdContact; private: class tri { public: int id; Vector3 p1, p2, p3; }; class col_tri { public: int status; // 0: unvisited, 1: visited, 2: included in the convex neighbor Vector3 p1, p2, p3; Vector3 n; }; static void copy_tri(col_tri* t1, tri* t2); static void copy_tri(col_tri* t1, col_tri* t2); static void calc_normal_vector(col_tri* t); static int is_convex_neighbor(col_tri* t1, col_tri* t2); static int identical_ver(const Vector3& v1, const Vector3& v2); static int is_neighboring_triangle(col_tri* t1, col_tri* t2); static void get_neighboring_triangles( col_tri* tri_convex_neighbor, col_tri* tri_neighbor, int* start_tri, int* end_tri, int num_tri); static int get_triangles_in_convex_neighbor( tri* root, col_tri* tri_convex_neighbor, col_tri* tri_neighbor, int num_tri, int max_num); static int get_triangles_in_convex_neighbor( tri* root, col_tri* tri_convex_neighbor, col_tri* tri_neighbor, int num_tri); static void get_triangles_in_neighbor( col_tri* neighbor_tris, int* n, const Opcode::AABBCollisionNode* root, Opcode::MeshInterface* mesh); static int count_num_of_triangles(const Opcode::AABBCollisionNode* root); void examine_normal_vector( const Opcode::AABBCollisionNode* b1, const Opcode::AABBCollisionNode* b2, int ctype, Opcode::MeshInterface* mesh1, Opcode::MeshInterface* mesh2); void check_separability( const Opcode::AABBCollisionNode* b1, const Opcode::AABBCollisionNode* root1, int num_tri1, const Opcode::AABBCollisionNode* b2, const Opcode::AABBCollisionNode* root2, int num_tri2, int ctype, Opcode::MeshInterface* mesh1, Opcode::MeshInterface* mesh2); void find_signed_distance( Vector3 &signed_distance, col_tri *trp, int nth, int ctype, int obj); void find_signed_distance( Vector3& signed_distance, const Vector3& vert, int nth, int ctype, int obj); void find_signed_distance( Vector3& signed_distance, const Opcode::AABBCollisionNode* b1, const Opcode::AABBCollisionNode* root, int num_tri, int contactIndex, int ctype, int obj, Opcode::MeshInterface* mesh); int new_point_test(int k); }; } #endif choreonoid-1.1.0+dfsg/src/Collision/DistFuncs.cpp000066400000000000000000000255111207742442300217220ustar00rootroot00000000000000#include "DistFuncs.h" #if 1 std::ostream &operator<<(std::ostream &ost, const Point& p) { ost << "(" << p.x << ", " << p.y << ", " << p.z << ")"; return ost; } #endif /** * @brief compute the minimum distance and the closest points between two line segments * * This function is implemented referring the following webpage * http://www.softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm * @param u0 start point of the first line segment * @param u vector from u0 to the other end point of the first line segment * @param v0 start point of the second line segment * @param v vector from v0 the other end point of the second line segment * @param cp0 the closest point on the first line segment * @param cp1 the closest point on the second line segment * @return the minimum distance */ inline float SegSegDist(const Point& u0, const Point& u, const Point& v0, const Point& v, Point& cp0, Point& cp1) { Point w = u0 - v0; float a = u|u; // always >= 0 float b = u|v; float c = v|v; // always >= 0 float d = u|w; float e = v|w; float D = a*c - b*b; // always >= 0 float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 // compute the line parameters of the two closest points #define EPS 1e-8 if (D < EPS*a*c) { // the lines are almost parallel sN = 0.0; // force using point P0 on segment S1 sD = 1.0; // to prevent possible division by 0.0 later tN = e; tD = c; } else { // get the closest points on the infinite lines sN = (b*e - c*d); tN = (a*e - b*d); if (sN < 0.0) { // sc < 0 => the s=0 edge is visible sN = 0.0; tN = e; tD = c; } else if (sN > sD) { // sc > 1 => the s=1 edge is visible sN = sD; tN = e + b; tD = c; } } if (tN < 0.0) { // tc < 0 => the t=0 edge is visible tN = 0.0; // recompute sc for this edge if (-d < 0.0) sN = 0.0; else if (-d > a) sN = sD; else { sN = -d; sD = a; } } else if (tN > tD) { // tc > 1 => the t=1 edge is visible tN = tD; // recompute sc for this edge if ((-d + b) < 0.0) sN = 0; else if ((-d + b) > a) sN = sD; else { sN = (-d + b); sD = a; } } // finally do the division to get sc and tc sc = (fabsf(sN) < EPS*a*c ? 0.0f : sN / sD); tc = (fabsf(tN) < EPS*a*c ? 0.0f : tN / tD); cp0 = u0 + sc * u; cp1 = v0 + tc * v; // get the difference of the two closest points Point dP = cp0 - cp1; return dP.Magnitude(); // return the closest distance } /** * @brief compute signed distance between a point and a plane * @param P a point * @param pointOnPolane a point on the plane * @param n normal vector of the plane * @param cp the closest point on the plane from P */ inline float PointPlaneDist(const Point& P, const Point& pointOnPlane, const Point& n, Point& cp) { Point v = P - pointOnPlane; float l = v|n; cp = P-l*n; return l; } // see DistFuncs.h float PointSegDist(const Point& P, const Point& u0, const Point& u1) { Point v = P - u0; Point dir = u1 - u0; float l = dir.Magnitude(); dir /= l; float x = v|dir; if (x < 0){ return v.Magnitude(); }else if (x > l){ Point dv = P - u1; return dv.Magnitude(); }else{ return sqrtf(v.SquareMagnitude() - x*x); } } /** * @brief check whether a point is in Voroni region of a face * @param p a point to be tested * @param vertices vertices of the triangle * @param edges edges of the triangle * @param n normal vector of the triangle * @return true if the point is in the Voronoi region, false otherwise */ inline bool PointFaceAppTest(const Point& P, const Point** vertices, const Point* edges, const Point& n) { Point v, outer; for (unsigned int i=0; i<3; i++){ v = P - *vertices[i]; outer = edges[i]^n; if ((v|outer)>0) return false; } return true; } // see DistFuncs.h float TriTriDist(const Point& U0, const Point& U1, const Point& U2, const Point& V0, const Point& V1, const Point& V2, Point& cp0, Point& cp1) { const Point* uvertices[] = {&U0, &U1, &U2}; const Point* vvertices[] = {&V0, &V1, &V2}; float min_d, d; Point p0, p1, n, vec; bool overlap = true; // edges Point uedges[3], vedges[3]; for (unsigned int i=0; i<3; i++){ uedges[i] = *uvertices[(i+1)%3] - *uvertices[i]; vedges[i] = *vvertices[(i+1)%3] - *vvertices[i]; } // set initial values cp0 = U0; cp1 = V0; min_d = (cp0-V0).Magnitude(); // vertex-vertex, vertex-edge, edge-edge for (unsigned int i=0; i<3; i++){ for (unsigned int j=0; j<3; j++){ d = SegSegDist(*uvertices[i], uedges[i], *vvertices[j], vedges[j], p0, p1); n = p0 - p1; if (d <= min_d) min_d = d; cp0 = p0; cp1 = p1; // check overlap vec = *uvertices[(i+2)%3] - cp0; float u = (vec|n)/min_d; vec = *vvertices[(j+2)%3] - cp1; float v = (vec|n)/min_d; // n directs from v -> u if (u>=0 && v<=0) return min_d; if (u > 0) u = 0; if (v < 0) v = 0; if ((n.Magnitude() + u - v) > 0){ overlap = false; } } } Point un = uedges[0]^uedges[1]; un.Normalize(); Point vn = vedges[0]^vedges[1]; vn.Normalize(); // vertex-face, edge-face, face-face // plane-triangle overlap test float ds[3]; int rank; // u and plane including v rank = -1; for (int i=0; i<3; i++){ vec = *uvertices[i] - *vvertices[0]; ds[i] = vec|vn; } if (ds[0] > 0 && ds[1] > 0 && ds[2] > 0){ // find rank of the nearest vertex to the plane rank = ds[0] > ds[1] ? 1 : 0; rank = ds[rank] > ds[2] ? 2 : rank; }else if(ds[0] < 0 && ds[1] < 0 && ds[2] < 0){ // find rank of the nearest vertex to the plane rank = ds[0] > ds[1] ? 0 : 1; rank = ds[rank] > ds[2] ? rank : 2; } if (rank >=0){ overlap = false; if (PointFaceAppTest(*uvertices[rank], vvertices, vedges, vn)){ min_d = fabsf(PointPlaneDist(*uvertices[rank], *vvertices[0], vn, p1)); cp0 = *uvertices[rank]; cp1 = p1; return min_d; } } // v and plane including u rank = -1; for (int i=0; i<3; i++){ vec = *vvertices[i] - *uvertices[0]; ds[i] = vec|un; } if (ds[0] > 0 && ds[1] > 0 && ds[2] > 0){ // find rank of the nearest vertex to the plane rank = ds[0] > ds[1] ? 1 : 0; rank = ds[rank] > ds[2] ? 2 : rank; }else if(ds[0] < 0 && ds[1] < 0 && ds[2] < 0){ // find rank of the nearest vertex to the plane rank = ds[0] > ds[1] ? 0 : 1; rank = ds[rank] > ds[2] ? rank : 2; } if (rank >= 0){ overlap = false; if (PointFaceAppTest(*vvertices[rank], uvertices, uedges, un)){ min_d = fabsf(PointPlaneDist(*vvertices[rank], *uvertices[0], un, p0)); cp0 = p0; cp1 = *vvertices[rank]; return min_d; } } if (overlap){ return 0; }else{ return min_d; } } // see DistFuncs.h float SegSegDist(const Point& u0, const Point& u1, const Point& v0, const Point& v1) { Point cp0, cp1; return SegSegDist(u0, u1-u0, v0, v1-v0, cp0, cp1); } #if 0 int main() { Point p0(0,0,0), p1(2,0,0), p2(1, 1, -1), p3(1, 1, 1); Point cp1, cp2, n; float d; d= SegSegDist(p0, p1, p2, p3, cp1, cp2); std::cout << "test1 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; Point p4(0,1,0), p5(2,1,0); d= SegSegDist(p0, p1, p4, p5, cp1, cp2); std::cout << "test2 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; d= SegSegDist(p0, p1, p0, p1, cp1, cp2); std::cout << "test3 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; Point p6(0, 2, 0), p7(-2, 1, 0); d = TriTriDist(p0, p1, p5, p4, p6, p7, cp1, cp2); std::cout << "test4 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; Point p8(3, -1, 1), p9(3, 2, 1), p10(-3, -1, 1); d = TriTriDist(p0, p1, p5, p8, p9, p10, cp1, cp2); std::cout << "test5 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; d = TriTriDist(p0, p1, p2, p8, p9, p10, cp1, cp2); std::cout << "test6 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; std::cout << "answer: d = 1, cp1 = (0, 0, 0), cp2 = (0, 0, 1)" << std::endl; d = TriTriDist(p2, p0, p1, p8, p9, p10, cp1, cp2); std::cout << "test7 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; std::cout << "answer: d = 1, cp1 = (0, 0, 0), cp2 = (0, 0, 1)" << std::endl; d = TriTriDist(p1, p2, p0, p8, p9, p10, cp1, cp2); std::cout << "test8 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; std::cout << "answer: d = 1, cp1 = (2, 0, 0), cp2 = (2, 0, 1)" << std::endl; { Point pp0(-2, 2, 0), pp1(-2, -2, 0), pp2(5, -2, 0), pp3(-0.1, 0.4, -0.1), pp4(-0.1, -0.4, -0.1), pp5(-0.1, -0.4, 0.1); d = TriTriDist(pp0, pp1, pp2, pp3, pp4, pp5, cp1, cp2); std::cout << "test9 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; std::cout << "answer: d = 0.0" << std::endl; } { Point pp0(-2, 2, 0), pp1(-2, -2, 0), pp2(5, -2, 0), pp3(-0.1, 0.4, -0.1), pp4(-0.1, -0.4, -0.1), pp5(-0.1, -0.4, 0.1); d = TriTriDist(pp0, pp1, pp2, pp3, pp4, pp5, cp1, cp2); std::cout << "test9 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; std::cout << "answer: d = 0.0" << std::endl; } { Point p0(0.1, 0.4, 0), p1(-0.1, 0.4, 0), p2(0.1, -0.4, 0); Point p3(0.05, 0.45, -0.15), p4(0.05, 0.45, 0.05), p5(0.05, -0.35, -0.15); d = TriTriDist(p0, p1, p2, p3, p4, p5, cp1, cp2); std::cout << "test10 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 << std::endl; std::cout << "answer: d = 0.0" << std::endl; } return 0; } #endif choreonoid-1.1.0+dfsg/src/Collision/DistFuncs.h000066400000000000000000000031211207742442300213600ustar00rootroot00000000000000 #include "Opcode/Opcode.h" /** * @brief compute distance between a point and a line segment * @param P the point * @param u0 one of end points of the line segment * @param u1 the other end point of the line segment * @return distance between the point and the line segment */ float PointSegDist(const Point& P, const Point& u0, const Point& u1); /** * @brief compute distance between line segments * @brief u0 one of end points of the first line segment * @brief u1 the other end point of the first line segment * @brief v0 one of end points of the second line segment * @brief v1 the other end point of the second line segment * @return distance between line segments */ float SegSegDist(const Point& u0, const Point& u1, const Point& v0, const Point& v1); /** * @brief compute the minimum distance and the closest points between two triangles * @param U0 the first vertex of the first triangle * @param U1 the second vertex of the first triangle * @param U2 the third vertex of the first triangle * @param V0 the first vertex of the second triangle * @param V1 the second vertex of the second triangle * @param V2 the third vertex of the second triangle * @param cp0 the closest point on the first triangle * @param cp1 the closest point on the second triangle * @return the minimum distance */ float TriTriDist(const Point& U0, const Point& U1, const Point& U2, const Point& V0, const Point& V1, const Point& V2, Point& cp0, Point& cp1); #if 1 #include std::ostream &operator<<(std::ostream &ost, const Point& p); #endif choreonoid-1.1.0+dfsg/src/Collision/Opcode/000077500000000000000000000000001207742442300205215ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/000077500000000000000000000000001207742442300212215ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceAABB.cpp000066400000000000000000000375621207742442300230500ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains AABB-related code. * \file IceAABB.cpp * \author Pierre Terdiman * \date January, 29, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * AABB class. * \class AABB * \author Pierre Terdiman * \version 1.0 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the sum of two AABBs. * \param aabb [in] the other AABB * \return Self-Reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABB& AABB::Add(const AABB& aabb) { // Compute new min & max values Point Min; GetMin(Min); Point Tmp; aabb.GetMin(Tmp); Min.Min(Tmp); Point Max; GetMax(Max); aabb.GetMax(Tmp); Max.Max(Tmp); // Update this SetMinMax(Min, Max); return *this; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Makes a cube from the AABB. * \param cube [out] the cube AABB * \return cube edge length */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float AABB::MakeCube(AABB& cube) const { Point Ext; GetExtents(Ext); float Max = Ext.Max(); Point Cnt; GetCenter(Cnt); cube.SetCenterExtents(Cnt, Point(Max, Max, Max)); return Max; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Makes a sphere from the AABB. * \param sphere [out] sphere containing the AABB */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABB::MakeSphere(Sphere& sphere) const { GetExtents(sphere.mCenter); sphere.mRadius = sphere.mCenter.Magnitude() * 1.00001f; // To make sure sphere::Contains(*this) succeeds GetCenter(sphere.mCenter); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks a box is inside another box. * \param box [in] the other AABB * \return true if current box is inside input box */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABB::IsInside(const AABB& box) const { if(box.GetMin(0)>GetMin(0)) return false; if(box.GetMin(1)>GetMin(1)) return false; if(box.GetMin(2)>GetMin(2)) return false; if(box.GetMax(0) max.x) ? 2 : 0) // 2 = right + ((local_eye.y < min.y) ? 4 : 0) // 4 = bottom + ((local_eye.y > max.y) ? 8 : 0) // 8 = top + ((local_eye.z < min.z) ? 16 : 0) // 16 = front + ((local_eye.z > max.z) ? 32 : 0); // 32 = back // Look up number of vertices in outline num = (sdword)gIndexList[pos][7]; // Zero indicates invalid case if(!num) return null; return &gIndexList[pos][0]; } // calculateBoxArea: computes the screen-projected 2D area of an oriented 3D bounding box //const Point& eye, //eye point (in bbox object coordinates) //const AABB& box, //3d bbox //const Matrix4x4& mat, //free transformation for bbox //float width, float height, int& num) float AABB::ComputeBoxArea(const Point& eye, const Matrix4x4& mat, float width, float height, sdword& num) const { const sbyte* Outline = ComputeOutline(eye, num); if(!Outline) return -1.0f; // Compute box vertices Point vertexBox[8], dst[8]; ComputePoints(vertexBox); // Transform all outline corners into 2D screen space for(sdword i=0;i GetMax(0) || p.x < GetMin(0)) return FALSE; \ if(p.y > GetMax(1) || p.y < GetMin(1)) return FALSE; \ if(p.z > GetMax(2) || p.z < GetMin(2)) return FALSE; \ return TRUE; \ } enum AABBType { AABB_RENDER = 0, //!< AABB used for rendering. Not visible == not rendered. AABB_UPDATE = 1, //!< AABB used for dynamic updates. Not visible == not updated. AABB_FORCE_DWORD = 0x7fffffff, }; #ifdef USE_MINMAX struct ICEMATHS_API ShadowAABB { Point mMin; Point mMax; }; class ICEMATHS_API AABB { public: //! Constructor inline_ AABB() {} //! Destructor inline_ ~AABB() {} //! Type-independent methods AABB_COMMON_METHODS; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups an AABB from min & max vectors. * \param min [in] the min point * \param max [in] the max point */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetMinMax(const Point& min, const Point& max) { mMin = min; mMax = max; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups an AABB from center & extents vectors. * \param c [in] the center point * \param e [in] the extents vector */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetCenterExtents(const Point& c, const Point& e) { mMin = c - e; mMax = c + e; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups an empty AABB. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetEmpty() { Point p(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); mMin = -p; mMax = p;} /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups a point AABB. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetPoint(const Point& pt) { mMin = mMax = pt; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the size of the AABB. The size is defined as the longest extent. * \return the size of the AABB */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float GetSize() const { Point e; GetExtents(e); return e.Max(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Extends the AABB. * \param p [in] the next point */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Extend(const Point& p) { if(p.x > mMax.x) mMax.x = p.x; if(p.x < mMin.x) mMin.x = p.x; if(p.y > mMax.y) mMax.y = p.y; if(p.y < mMin.y) mMin.y = p.y; if(p.z > mMax.z) mMax.z = p.z; if(p.z < mMin.z) mMin.z = p.z; } // Data access //! Get min point of the box inline_ void GetMin(Point& min) const { min = mMin; } //! Get max point of the box inline_ void GetMax(Point& max) const { max = mMax; } //! Get component of the box's min point along a given axis inline_ float GetMin(udword axis) const { return mMin[axis]; } //! Get component of the box's max point along a given axis inline_ float GetMax(udword axis) const { return mMax[axis]; } //! Get box center inline_ void GetCenter(Point& center) const { center = (mMax + mMin)*0.5f; } //! Get box extents inline_ void GetExtents(Point& extents) const { extents = (mMax - mMin)*0.5f; } //! Get component of the box's center along a given axis inline_ float GetCenter(udword axis) const { return (mMax[axis] + mMin[axis])*0.5f; } //! Get component of the box's extents along a given axis inline_ float GetExtents(udword axis) const { return (mMax[axis] - mMin[axis])*0.5f; } //! Get box diagonal inline_ void GetDiagonal(Point& diagonal) const { diagonal = mMax - mMin; } inline_ float GetWidth() const { return mMax.x - mMin.x; } inline_ float GetHeight() const { return mMax.y - mMin.y; } inline_ float GetDepth() const { return mMax.z - mMin.z; } //! Volume inline_ float GetVolume() const { return GetWidth() * GetHeight() * GetDepth(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the intersection between two AABBs. * \param a [in] the other AABB * \return true on intersection */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL Intersect(const AABB& a) const { if(mMax.x < a.mMin.x || a.mMax.x < mMin.x || mMax.y < a.mMin.y || a.mMax.y < mMin.y || mMax.z < a.mMin.z || a.mMax.z < mMin.z) return FALSE; return TRUE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the 1D-intersection between two AABBs, on a given axis. * \param a [in] the other AABB * \param axis [in] the axis (0, 1, 2) * \return true on intersection */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL Intersect(const AABB& a, udword axis) const { if(mMax[axis] < a.mMin[axis] || a.mMax[axis] < mMin[axis]) return FALSE; return TRUE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recomputes the AABB after an arbitrary transform by a 4x4 matrix. * Original code by Charles Bloom on the GD-Algorithm list. (I slightly modified it) * \param mtx [in] the transform matrix * \param aabb [out] the transformed AABB [can be *this] */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void Rotate(const Matrix4x4& mtx, AABB& aabb) const { // The three edges transformed: you can efficiently transform an X-only vector // by just getting the "X" column of the matrix Point vx,vy,vz; mtx.GetRow(0, vx); vx *= (mMax.x - mMin.x); mtx.GetRow(1, vy); vy *= (mMax.y - mMin.y); mtx.GetRow(2, vz); vz *= (mMax.z - mMin.z); // Transform the min point aabb.mMin = aabb.mMax = mMin * mtx; // Take the transformed min & axes and find new extents // Using CPU code in the right place is faster... if(IS_NEGATIVE_FLOAT(vx.x)) aabb.mMin.x += vx.x; else aabb.mMax.x += vx.x; if(IS_NEGATIVE_FLOAT(vx.y)) aabb.mMin.y += vx.y; else aabb.mMax.y += vx.y; if(IS_NEGATIVE_FLOAT(vx.z)) aabb.mMin.z += vx.z; else aabb.mMax.z += vx.z; if(IS_NEGATIVE_FLOAT(vy.x)) aabb.mMin.x += vy.x; else aabb.mMax.x += vy.x; if(IS_NEGATIVE_FLOAT(vy.y)) aabb.mMin.y += vy.y; else aabb.mMax.y += vy.y; if(IS_NEGATIVE_FLOAT(vy.z)) aabb.mMin.z += vy.z; else aabb.mMax.z += vy.z; if(IS_NEGATIVE_FLOAT(vz.x)) aabb.mMin.x += vz.x; else aabb.mMax.x += vz.x; if(IS_NEGATIVE_FLOAT(vz.y)) aabb.mMin.y += vz.y; else aabb.mMax.y += vz.y; if(IS_NEGATIVE_FLOAT(vz.z)) aabb.mMin.z += vz.z; else aabb.mMax.z += vz.z; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the AABB is valid. * \return true if the box is valid */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL IsValid() const { // Consistency condition for (Min, Max) boxes: min < max if(mMin.x > mMax.x) return FALSE; if(mMin.y > mMax.y) return FALSE; if(mMin.z > mMax.z) return FALSE; return TRUE; } //! Operator for AABB *= float. Scales the extents, keeps same center. inline_ AABB& operator*=(float s) { Point Center; GetCenter(Center); Point Extents; GetExtents(Extents); SetCenterExtents(Center, Extents * s); return *this; } //! Operator for AABB /= float. Scales the extents, keeps same center. inline_ AABB& operator/=(float s) { Point Center; GetCenter(Center); Point Extents; GetExtents(Extents); SetCenterExtents(Center, Extents / s); return *this; } //! Operator for AABB += Point. Translates the box. inline_ AABB& operator+=(const Point& trans) { mMin+=trans; mMax+=trans; return *this; } private: Point mMin; //!< Min point Point mMax; //!< Max point }; #else class ICEMATHS_API AABB { public: //! Constructor inline_ AABB() {} //! Destructor inline_ ~AABB() {} //! Type-independent methods AABB_COMMON_METHODS; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups an AABB from min & max vectors. * \param min [in] the min point * \param max [in] the max point */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetMinMax(const Point& min, const Point& max) { mCenter = (max + min)*0.5f; mExtents = (max - min)*0.5f; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups an AABB from center & extents vectors. * \param c [in] the center point * \param e [in] the extents vector */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetCenterExtents(const Point& c, const Point& e) { mCenter = c; mExtents = e; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups an empty AABB. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetEmpty() { mCenter.Zero(); mExtents.Set(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);} /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups a point AABB. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetPoint(const Point& pt) { mCenter = pt; mExtents.Zero(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the size of the AABB. The size is defined as the longest extent. * \return the size of the AABB */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float GetSize() const { return mExtents.Max(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Extends the AABB. * \param p [in] the next point */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Extend(const Point& p) { Point Max = mCenter + mExtents; Point Min = mCenter - mExtents; if(p.x > Max.x) Max.x = p.x; if(p.x < Min.x) Min.x = p.x; if(p.y > Max.y) Max.y = p.y; if(p.y < Min.y) Min.y = p.y; if(p.z > Max.z) Max.z = p.z; if(p.z < Min.z) Min.z = p.z; SetMinMax(Min, Max); } // Data access //! Get min point of the box inline_ void GetMin(Point& min) const { min = mCenter - mExtents; } //! Get max point of the box inline_ void GetMax(Point& max) const { max = mCenter + mExtents; } //! Get component of the box's min point along a given axis inline_ float GetMin(udword axis) const { return mCenter[axis] - mExtents[axis]; } //! Get component of the box's max point along a given axis inline_ float GetMax(udword axis) const { return mCenter[axis] + mExtents[axis]; } //! Get box center inline_ void GetCenter(Point& center) const { center = mCenter; } //! Get box extents inline_ void GetExtents(Point& extents) const { extents = mExtents; } //! Get component of the box's center along a given axis inline_ float GetCenter(udword axis) const { return mCenter[axis]; } //! Get component of the box's extents along a given axis inline_ float GetExtents(udword axis) const { return mExtents[axis]; } //! Get box diagonal inline_ void GetDiagonal(Point& diagonal) const { diagonal = mExtents * 2.0f; } inline_ float GetWidth() const { return mExtents.x * 2.0f; } inline_ float GetHeight() const { return mExtents.y * 2.0f; } inline_ float GetDepth() const { return mExtents.z * 2.0f; } //! Volume inline_ float GetVolume() const { return mExtents.x * mExtents.y * mExtents.z * 8.0f; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the intersection between two AABBs. * \param a [in] the other AABB * \return true on intersection */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL Intersect(const AABB& a) const { float tx = mCenter.x - a.mCenter.x; float ex = a.mExtents.x + mExtents.x; if(AIR(tx) > IR(ex)) return FALSE; float ty = mCenter.y - a.mCenter.y; float ey = a.mExtents.y + mExtents.y; if(AIR(ty) > IR(ey)) return FALSE; float tz = mCenter.z - a.mCenter.z; float ez = a.mExtents.z + mExtents.z; if(AIR(tz) > IR(ez)) return FALSE; return TRUE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * The standard intersection method from Gamasutra. Just here to check its speed against the one above. * \param a [in] the other AABB * \return true on intersection */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ bool GomezIntersect(const AABB& a) { Point T = mCenter - a.mCenter; // Vector from A to B return ((fabsf(T.x) <= (a.mExtents.x + mExtents.x)) && (fabsf(T.y) <= (a.mExtents.y + mExtents.y)) && (fabsf(T.z) <= (a.mExtents.z + mExtents.z))); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the 1D-intersection between two AABBs, on a given axis. * \param a [in] the other AABB * \param axis [in] the axis (0, 1, 2) * \return true on intersection */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL Intersect(const AABB& a, udword axis) const { float t = mCenter[axis] - a.mCenter[axis]; float e = a.mExtents[axis] + mExtents[axis]; if(AIR(t) > IR(e)) return FALSE; return TRUE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recomputes the AABB after an arbitrary transform by a 4x4 matrix. * \param mtx [in] the transform matrix * \param aabb [out] the transformed AABB [can be *this] */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void Rotate(const Matrix4x4& mtx, AABB& aabb) const { // Compute new center aabb.mCenter = mCenter * mtx; // Compute new extents. FPU code & CPU code have been interleaved for improved performance. Point Ex(mtx.m[0][0] * mExtents.x, mtx.m[0][1] * mExtents.x, mtx.m[0][2] * mExtents.x); IR(Ex.x)&=0x7fffffff; IR(Ex.y)&=0x7fffffff; IR(Ex.z)&=0x7fffffff; Point Ey(mtx.m[1][0] * mExtents.y, mtx.m[1][1] * mExtents.y, mtx.m[1][2] * mExtents.y); IR(Ey.x)&=0x7fffffff; IR(Ey.y)&=0x7fffffff; IR(Ey.z)&=0x7fffffff; Point Ez(mtx.m[2][0] * mExtents.z, mtx.m[2][1] * mExtents.z, mtx.m[2][2] * mExtents.z); IR(Ez.x)&=0x7fffffff; IR(Ez.y)&=0x7fffffff; IR(Ez.z)&=0x7fffffff; aabb.mExtents.x = Ex.x + Ey.x + Ez.x; aabb.mExtents.y = Ex.y + Ey.y + Ez.y; aabb.mExtents.z = Ex.z + Ey.z + Ez.z; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the AABB is valid. * \return true if the box is valid */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL IsValid() const { // Consistency condition for (Center, Extents) boxes: Extents >= 0 if(IS_NEGATIVE_FLOAT(mExtents.x)) return FALSE; if(IS_NEGATIVE_FLOAT(mExtents.y)) return FALSE; if(IS_NEGATIVE_FLOAT(mExtents.z)) return FALSE; return TRUE; } //! Operator for AABB *= float. Scales the extents, keeps same center. inline_ AABB& operator*=(float s) { mExtents*=s; return *this; } //! Operator for AABB /= float. Scales the extents, keeps same center. inline_ AABB& operator/=(float s) { mExtents/=s; return *this; } //! Operator for AABB += Point. Translates the box. inline_ AABB& operator+=(const Point& trans) { mCenter+=trans; return *this; } private: Point mCenter; //!< AABB Center Point mExtents; //!< x, y and z extents }; #endif inline_ void ComputeMinMax(const Point& p, Point& min, Point& max) { if(p.x > max.x) max.x = p.x; if(p.x < min.x) min.x = p.x; if(p.y > max.y) max.y = p.y; if(p.y < min.y) min.y = p.y; if(p.z > max.z) max.z = p.z; if(p.z < min.z) min.z = p.z; } inline_ void ComputeAABB(AABB& aabb, const Point* list, udword nb_pts) { if(list) { Point Maxi(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); Point Mini(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT); while(nb_pts--) { // _prefetch(list+1); // off by one ? ComputeMinMax(*list++, Mini, Maxi); } aabb.SetMinMax(Mini, Maxi); } } #endif // __ICEAABB_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceAxes.h000066400000000000000000000026721207742442300227220ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains axes definition. * \file IceAxes.h * \author Pierre Terdiman * \date January, 29, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEAXES_H__ #define __ICEAXES_H__ enum PointComponent { _X = 0, _Y = 1, _Z = 2, _W = 3, _FORCE_DWORD = 0x7fffffff }; enum AxisOrder { AXES_XYZ = (_X)|(_Y<<2)|(_Z<<4), AXES_XZY = (_X)|(_Z<<2)|(_Y<<4), AXES_YXZ = (_Y)|(_X<<2)|(_Z<<4), AXES_YZX = (_Y)|(_Z<<2)|(_X<<4), AXES_ZXY = (_Z)|(_X<<2)|(_Y<<4), AXES_ZYX = (_Z)|(_Y<<2)|(_X<<4), AXES_FORCE_DWORD = 0x7fffffff }; class ICEMATHS_API Axes { public: inline_ Axes(AxisOrder order) { mAxis0 = (order ) & 3; mAxis1 = (order>>2) & 3; mAxis2 = (order>>4) & 3; } inline_ ~Axes() {} udword mAxis0; udword mAxis1; udword mAxis2; }; #endif // __ICEAXES_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceBoundingSphere.h000066400000000000000000000154061207742442300247350ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code to compute the minimal bounding sphere. * \file IceBoundingSphere.h * \author Pierre Terdiman * \date January, 29, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEBOUNDINGSPHERE_H__ #define __ICEBOUNDINGSPHERE_H__ enum BSphereMethod { BS_NONE, BS_GEMS, BS_MINIBALL, BS_FORCE_DWORD = 0x7fffffff }; class ICEMATHS_API Sphere { public: //! Constructor inline_ Sphere() {} //! Constructor inline_ Sphere(const Point& center, float radius) : mCenter(center), mRadius(radius) {} //! Constructor Sphere(udword nb_verts, const Point* verts); //! Copy constructor inline_ Sphere(const Sphere& sphere) : mCenter(sphere.mCenter), mRadius(sphere.mRadius) {} //! Destructor inline_ ~Sphere() {} BSphereMethod Compute(udword nb_verts, const Point* verts); bool FastCompute(udword nb_verts, const Point* verts); // Access methods inline_ const Point& GetCenter() const { return mCenter; } inline_ float GetRadius() const { return mRadius; } inline_ const Point& Center() const { return mCenter; } inline_ float Radius() const { return mRadius; } inline_ Sphere& Set(const Point& center, float radius) { mCenter = center; mRadius = radius; return *this; } inline_ Sphere& SetCenter(const Point& center) { mCenter = center; return *this; } inline_ Sphere& SetRadius(float radius) { mRadius = radius; return *this; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if a point is contained within the sphere. * \param p [in] the point to test * \return true if inside the sphere */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ bool Contains(const Point& p) const { return mCenter.SquareDistance(p) <= mRadius*mRadius; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if a sphere is contained within the sphere. * \param sphere [in] the sphere to test * \return true if inside the sphere */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ bool Contains(const Sphere& sphere) const { // If our radius is the smallest, we can't possibly contain the other sphere if(mRadius < sphere.mRadius) return false; // So r is always positive or null now float r = mRadius - sphere.mRadius; return mCenter.SquareDistance(sphere.mCenter) <= r*r; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if a box is contained within the sphere. * \param aabb [in] the box to test * \return true if inside the sphere */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL Contains(const AABB& aabb) const { // I assume if all 8 box vertices are inside the sphere, so does the whole box. // Sounds ok but maybe there's a better way? float R2 = mRadius * mRadius; #ifdef USE_MIN_MAX const Point& Max = ((ShadowAABB&)&aabb).mMax; const Point& Min = ((ShadowAABB&)&aabb).mMin; #else Point Max; aabb.GetMax(Max); Point Min; aabb.GetMin(Min); #endif Point p; p.x=Max.x; p.y=Max.y; p.z=Max.z; if(mCenter.SquareDistance(p)>=R2) return FALSE; p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE; p.x=Max.x; p.y=Min.y; if(mCenter.SquareDistance(p)>=R2) return FALSE; p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE; p.x=Max.x; p.y=Max.y; p.z=Min.z; if(mCenter.SquareDistance(p)>=R2) return FALSE; p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE; p.x=Max.x; p.y=Min.y; if(mCenter.SquareDistance(p)>=R2) return FALSE; p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE; return TRUE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if the sphere intersects another sphere * \param sphere [in] the other sphere * \return true if spheres overlap */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ bool Intersect(const Sphere& sphere) const { float r = mRadius + sphere.mRadius; return mCenter.SquareDistance(sphere.mCenter) <= r*r; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the sphere is valid. * \return true if the box is valid */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL IsValid() const { // Consistency condition for spheres: Radius >= 0.0f if(mRadius < 0.0f) return FALSE; return TRUE; } public: Point mCenter; //!< Sphere center float mRadius; //!< Sphere radius }; #endif // __ICEBOUNDINGSPHERE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceContainer.cpp000066400000000000000000000332321207742442300242730ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a simple container class. * \file IceContainer.cpp * \author Pierre Terdiman * \date February, 5, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a list of 32-bits values. * Use this class when you need to store an unknown number of values. The list is automatically * resized and can contains 32-bits entities (dwords or floats) * * \class Container * \author Pierre Terdiman * \version 1.0 * \date 08.15.98 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceCore; // Static members #ifdef CONTAINER_STATS udword Container::mNbContainers = 0; udword Container::mUsedRam = 0; #endif /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. No entries allocated there. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Container::Container() : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(2.0f) { #ifdef CONTAINER_STATS mNbContainers++; mUsedRam+=sizeof(Container); #endif } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. Also allocates a given number of entries. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Container::Container(udword size, float growth_factor) : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(growth_factor) { #ifdef CONTAINER_STATS mNbContainers++; mUsedRam+=sizeof(Container); #endif SetSize(size); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Copy constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Container::Container(const Container& object) : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(2.0f) { #ifdef CONTAINER_STATS mNbContainers++; mUsedRam+=sizeof(Container); #endif *this = object; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. Frees everything and leaves. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Container::~Container() { Empty(); #ifdef CONTAINER_STATS mNbContainers--; mUsedRam-=GetUsedRam(); #endif } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Clears the container. All stored values are deleted, and it frees used ram. * \see Reset() * \return Self-Reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Container& Container::Empty() { #ifdef CONTAINER_STATS mUsedRam-=mMaxNbEntries*sizeof(udword); #endif DELETEARRAY(mEntries); mCurNbEntries = mMaxNbEntries = 0; return *this; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Resizes the container. * \param needed [in] assume the container can be added at least "needed" values * \return true if success. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Container::Resize(udword needed) { #ifdef CONTAINER_STATS // Subtract previous amount of bytes mUsedRam-=mMaxNbEntries*sizeof(udword); #endif // Get more entries mMaxNbEntries = mMaxNbEntries ? udword(float(mMaxNbEntries)*mGrowthFactor) : 2; // Default nb Entries = 2 if(mMaxNbEntriesmMaxNbEntries) Resize(nb); // Add new entry CopyMemory(&mEntries[mCurNbEntries], entries, nb*sizeof(udword)); mCurNbEntries+=nb; return *this; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A O(1) method to add a value in the container. The container is automatically resized if needed. * The method is inline, not the resize. The call overhead happens on resizes only, which is not a problem since the resizing operation * costs a lot more than the call overhead... * * \param entry [in] a float to store in the container * \see Add(udword entry) * \see Empty() * \see Contains(udword entry) * \return Self-Reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ Container& Add(float entry) { // Resize if needed if(mCurNbEntries==mMaxNbEntries) Resize(); // Add new entry mEntries[mCurNbEntries++] = IR(entry); return *this; } inline_ Container& Add(const float* entries, udword nb) { // Resize if needed if(mCurNbEntries+nb>mMaxNbEntries) Resize(nb); // Add new entry CopyMemory(&mEntries[mCurNbEntries], entries, nb*sizeof(float)); mCurNbEntries+=nb; return *this; } //! Add unique [slow] inline_ Container& AddUnique(udword entry) { if(!Contains(entry)) Add(entry); return *this; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Clears the container. All stored values are deleted, and it frees used ram. * \see Reset() * \return Self-Reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Container& Empty(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Resets the container. Stored values are discarded but the buffer is kept so that further calls don't need resizing again. * That's a kind of temporal coherence. * \see Empty() */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void Reset() { // Avoid the write if possible // ### CMOV if(mCurNbEntries) mCurNbEntries = 0; } // HANDLE WITH CARE inline_ void ForceSize(udword size) { mCurNbEntries = size; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Sets the initial size of the container. If it already contains something, it's discarded. * \param nb [in] Number of entries * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool SetSize(udword nb); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the container and get rid of unused bytes. * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Refit(); // Checks whether the container already contains a given value. bool Contains(udword entry, udword* location=null) const; // Deletes an entry - doesn't preserve insertion order. bool Delete(udword entry); // Deletes an entry - does preserve insertion order. bool DeleteKeepingOrder(udword entry); //! Deletes the very last entry. inline_ void DeleteLastEntry() { if(mCurNbEntries) mCurNbEntries--; } //! Deletes the entry whose index is given inline_ void DeleteIndex(udword index) { mEntries[index] = mEntries[--mCurNbEntries]; } // Helpers Container& FindNext(udword& entry, FindMode find_mode=FIND_CLAMP); Container& FindPrev(udword& entry, FindMode find_mode=FIND_CLAMP); // Data access. inline_ udword GetNbEntries() const { return mCurNbEntries; } //!< Returns the current number of entries. inline_ udword GetEntry(udword i) const { return mEntries[i]; } //!< Returns ith entry inline_ udword* GetEntries() const { return mEntries; } //!< Returns the list of entries. inline_ udword GetFirst() const { return mEntries[0]; } inline_ udword GetLast() const { return mEntries[mCurNbEntries-1]; } // Growth control inline_ float GetGrowthFactor() const { return mGrowthFactor; } //!< Returns the growth factor inline_ void SetGrowthFactor(float growth) { mGrowthFactor = growth; } //!< Sets the growth factor inline_ bool IsFull() const { return mCurNbEntries==mMaxNbEntries; } //!< Checks the container is full inline_ BOOL IsNotEmpty() const { return mCurNbEntries; } //!< Checks the container is empty //! Read-access as an array inline_ udword operator[](udword i) const { ASSERT(i>=0 && i=0 && i>31); return (float&)y; } //! Computes 1.0f / sqrtf(x). inline_ float frsqrt(float f) { float x = f * 0.5f; udword y = 0x5f3759df - ((udword&)f >> 1); // Iteration... (float&)y = (float&)y * ( 1.5f - ( x * (float&)y * (float&)y ) ); // Result return (float&)y; } //! Computes 1.0f / sqrtf(x). Comes from NVIDIA. inline_ float InvSqrt(const float& x) { udword tmp = (udword(IEEE_1_0 << 1) + IEEE_1_0 - *(udword*)&x) >> 1; float y = *(float*)&tmp; return y * (1.47f - 0.47f * x * y * y); } //! Computes 1.0f / sqrtf(x). Comes from Quake3. Looks like the first one I had above. //! See http://www.magic-software.com/3DGEDInvSqrt.html inline_ float RSqrt(float number) { long i; float x2, y; const float threehalfs = 1.5f; x2 = number * 0.5f; y = number; i = * (long *) &y; i = 0x5f3759df - (i >> 1); y = * (float *) &i; y = y * (threehalfs - (x2 * y * y)); return y; } //! TO BE DOCUMENTED inline_ float fsqrt(float f) { udword y = ( ( (sdword&)f - 0x3f800000 ) >> 1 ) + 0x3f800000; // Iteration...? // (float&)y = (3.0f - ((float&)y * (float&)y) / f) * (float&)y * 0.5f; // Result return (float&)y; } //! Returns the float ranged espilon value. inline_ float fepsilon(float f) { udword b = (udword&)f & 0xff800000; udword a = b | 0x00000001; (float&)a -= (float&)b; // Result return (float&)a; } //! Is the float valid ? inline_ bool IsNAN(float value) { return (IR(value)&0x7f800000) == 0x7f800000; } inline_ bool IsIndeterminate(float value) { return IR(value) == 0xffc00000; } inline_ bool IsPlusInf(float value) { return IR(value) == 0x7f800000; } inline_ bool IsMinusInf(float value) { return IR(value) == 0xff800000; } inline_ bool IsValidFloat(float value) { if(IsNAN(value)) return false; if(IsIndeterminate(value)) return false; if(IsPlusInf(value)) return false; if(IsMinusInf(value)) return false; return true; } #define CHECK_VALID_FLOAT(x) ASSERT(IsValidFloat(x)); /* //! FPU precision setting function. inline_ void SetFPU() { // This function evaluates whether the floating-point // control word is set to single precision/round to nearest/ // exceptions disabled. If these conditions don't hold, the // function changes the control word to set them and returns // TRUE, putting the old control word value in the passback // location pointed to by pwOldCW. { uword wTemp, wSave; __asm fstcw wSave if (wSave & 0x300 || // Not single mode 0x3f != (wSave & 0x3f) || // Exceptions enabled wSave & 0xC00) // Not round to nearest mode { __asm { mov ax, wSave and ax, not 300h ;; single mode or ax, 3fh ;; disable all exceptions and ax, not 0xC00 ;; round to nearest mode mov wTemp, ax fldcw wTemp } } } } */ //! This function computes the slowest possible floating-point value (you can also directly use FLT_EPSILON) inline_ float ComputeFloatEpsilon() { float f = 1.0f; ((udword&)f)^=1; return f - 1.0f; // You can check it's the same as FLT_EPSILON } inline_ bool IsFloatZero(float x, float epsilon=1e-6f) { return x*x < epsilon; } #define FCOMI_ST0 _asm _emit 0xdb _asm _emit 0xf0 #define FCOMIP_ST0 _asm _emit 0xdf _asm _emit 0xf0 #define FCMOVB_ST0 _asm _emit 0xda _asm _emit 0xc0 #define FCMOVNB_ST0 _asm _emit 0xdb _asm _emit 0xc0 #define FCOMI_ST1 _asm _emit 0xdb _asm _emit 0xf1 #define FCOMIP_ST1 _asm _emit 0xdf _asm _emit 0xf1 #define FCMOVB_ST1 _asm _emit 0xda _asm _emit 0xc1 #define FCMOVNB_ST1 _asm _emit 0xdb _asm _emit 0xc1 #define FCOMI_ST2 _asm _emit 0xdb _asm _emit 0xf2 #define FCOMIP_ST2 _asm _emit 0xdf _asm _emit 0xf2 #define FCMOVB_ST2 _asm _emit 0xda _asm _emit 0xc2 #define FCMOVNB_ST2 _asm _emit 0xdb _asm _emit 0xc2 #define FCOMI_ST3 _asm _emit 0xdb _asm _emit 0xf3 #define FCOMIP_ST3 _asm _emit 0xdf _asm _emit 0xf3 #define FCMOVB_ST3 _asm _emit 0xda _asm _emit 0xc3 #define FCMOVNB_ST3 _asm _emit 0xdb _asm _emit 0xc3 #define FCOMI_ST4 _asm _emit 0xdb _asm _emit 0xf4 #define FCOMIP_ST4 _asm _emit 0xdf _asm _emit 0xf4 #define FCMOVB_ST4 _asm _emit 0xda _asm _emit 0xc4 #define FCMOVNB_ST4 _asm _emit 0xdb _asm _emit 0xc4 #define FCOMI_ST5 _asm _emit 0xdb _asm _emit 0xf5 #define FCOMIP_ST5 _asm _emit 0xdf _asm _emit 0xf5 #define FCMOVB_ST5 _asm _emit 0xda _asm _emit 0xc5 #define FCMOVNB_ST5 _asm _emit 0xdb _asm _emit 0xc5 #define FCOMI_ST6 _asm _emit 0xdb _asm _emit 0xf6 #define FCOMIP_ST6 _asm _emit 0xdf _asm _emit 0xf6 #define FCMOVB_ST6 _asm _emit 0xda _asm _emit 0xc6 #define FCMOVNB_ST6 _asm _emit 0xdb _asm _emit 0xc6 #define FCOMI_ST7 _asm _emit 0xdb _asm _emit 0xf7 #define FCOMIP_ST7 _asm _emit 0xdf _asm _emit 0xf7 #define FCMOVB_ST7 _asm _emit 0xda _asm _emit 0xc7 #define FCMOVNB_ST7 _asm _emit 0xdb _asm _emit 0xc7 //! A global function to find MAX(a,b) using FCOMI/FCMOV inline_ float FCMax2(float a, float b) { #if defined(_MSC_VER) && !defined(_M_X64) float Res; _asm fld [a] _asm fld [b] FCOMI_ST1 FCMOVB_ST1 _asm fstp [Res] _asm fcomp return Res; #else return (a > b) ? a : b; #endif } //! A global function to find MIN(a,b) using FCOMI/FCMOV inline_ float FCMin2(float a, float b) { #if defined(_MSC_VER) && !defined(_M_X64) float Res; _asm fld [a] _asm fld [b] FCOMI_ST1 FCMOVNB_ST1 _asm fstp [Res] _asm fcomp return Res; #else return (a < b) ? a : b; #endif } //! A global function to find MAX(a,b,c) using FCOMI/FCMOV inline_ float FCMax3(float a, float b, float c) { #if defined(_MSC_VER) && !defined(_M_X64) float Res; _asm fld [a] _asm fld [b] _asm fld [c] FCOMI_ST1 FCMOVB_ST1 FCOMI_ST2 FCMOVB_ST2 _asm fstp [Res] _asm fcompp return Res; #else return (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c); #endif } //! A global function to find MIN(a,b,c) using FCOMI/FCMOV inline_ float FCMin3(float a, float b, float c) { #if defined(_MSC_VER) && !defined(_M_X64) float Res; _asm fld [a] _asm fld [b] _asm fld [c] FCOMI_ST1 FCMOVNB_ST1 FCOMI_ST2 FCMOVNB_ST2 _asm fstp [Res] _asm fcompp return Res; #else return (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c); #endif } inline_ int ConvertToSortable(float f) { int& Fi = (int&)f; int Fmask = (Fi>>31); Fi ^= Fmask; Fmask &= ~(1<<31); Fi -= Fmask; return Fi; } enum FPUMode { FPU_FLOOR = 0, FPU_CEIL = 1, FPU_BEST = 2, FPU_FORCE_DWORD = 0x7fffffff }; FUNCTION ICECORE_API FPUMode GetFPUMode(); FUNCTION ICECORE_API void SaveFPU(); FUNCTION ICECORE_API void RestoreFPU(); FUNCTION ICECORE_API void SetFPUFloorMode(); FUNCTION ICECORE_API void SetFPUCeilMode(); FUNCTION ICECORE_API void SetFPUBestMode(); FUNCTION ICECORE_API void SetFPUPrecision24(); FUNCTION ICECORE_API void SetFPUPrecision53(); FUNCTION ICECORE_API void SetFPUPrecision64(); FUNCTION ICECORE_API void SetFPURoundingChop(); FUNCTION ICECORE_API void SetFPURoundingUp(); FUNCTION ICECORE_API void SetFPURoundingDown(); FUNCTION ICECORE_API void SetFPURoundingNear(); FUNCTION ICECORE_API int intChop(const float& f); FUNCTION ICECORE_API int intFloor(const float& f); FUNCTION ICECORE_API int intCeil(const float& f); #endif // __ICEFPU_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceHPoint.cpp000066400000000000000000000075541207742442300235620ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for homogeneous points. * \file IceHPoint.cpp * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Homogeneous point. * * Use it: * - for clipping in homogeneous space (standard way) * - to differentiate between points (w=1) and vectors (w=0). * - in some cases you can also use it instead of Point for padding reasons. * * \class HPoint * \author Pierre Terdiman * \version 1.0 * \warning No cross-product in 4D. * \warning HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Point Mul = HPoint * Matrix3x3; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Point HPoint::operator*(const Matrix3x3& mat) const { return Point( x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0], x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1], x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] ); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // HPoint Mul = HPoint * Matrix4x4; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// HPoint HPoint::operator*(const Matrix4x4& mat) const { return HPoint( x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0], x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1], x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2], x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3]); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // HPoint *= Matrix4x4 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// HPoint& HPoint::operator*=(const Matrix4x4& mat) { float xp = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0]; float yp = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1]; float zp = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2]; float wp = x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3]; x = xp; y = yp; z = zp; w = wp; return *this; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceHPoint.h000066400000000000000000000163621207742442300232240ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for homogeneous points. * \file IceHPoint.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEHPOINT_H__ #define __ICEHPOINT_H__ class ICEMATHS_API HPoint : public Point { public: //! Empty constructor inline_ HPoint() {} //! Constructor from floats inline_ HPoint(float _x, float _y, float _z, float _w=0.0f) : Point(_x, _y, _z), w(_w) {} //! Constructor from array inline_ HPoint(const float f[4]) : Point(f), w(f[3]) {} //! Constructor from a Point inline_ HPoint(const Point& p, float _w=0.0f) : Point(p), w(_w) {} //! Destructor inline_ ~HPoint() {} //! Clear the point inline_ HPoint& Zero() { x = y = z = w = 0.0f; return *this; } //! Assignment from values inline_ HPoint& Set(float _x, float _y, float _z, float _w ) { x = _x; y = _y; z = _z; w = _w; return *this; } //! Assignment from array inline_ HPoint& Set(const float f[4]) { x = f[_X]; y = f[_Y]; z = f[_Z]; w = f[_W]; return *this; } //! Assignment from another h-point inline_ HPoint& Set(const HPoint& src) { x = src.x; y = src.y; z = src.z; w = src.w; return *this; } //! Add a vector inline_ HPoint& Add(float _x, float _y, float _z, float _w ) { x += _x; y += _y; z += _z; w += _w; return *this; } //! Add a vector inline_ HPoint& Add(const float f[4]) { x += f[_X]; y += f[_Y]; z += f[_Z]; w += f[_W]; return *this; } //! Subtract a vector inline_ HPoint& Sub(float _x, float _y, float _z, float _w ) { x -= _x; y -= _y; z -= _z; w -= _w; return *this; } //! Subtract a vector inline_ HPoint& Sub(const float f[4]) { x -= f[_X]; y -= f[_Y]; z -= f[_Z]; w -= f[_W]; return *this; } //! Multiplies by a scalar inline_ HPoint& Mul(float s) { x *= s; y *= s; z *= s; w *= s; return *this; } //! Returns MIN(x, y, z, w); float Min() const { return MIN(x, MIN(y, MIN(z, w))); } //! Returns MAX(x, y, z, w); float Max() const { return MAX(x, MAX(y, MAX(z, w))); } //! Sets each element to be componentwise minimum HPoint& Min(const HPoint& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); w = MIN(w, p.w); return *this; } //! Sets each element to be componentwise maximum HPoint& Max(const HPoint& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); w = MAX(w, p.w); return *this; } //! Computes square magnitude inline_ float SquareMagnitude() const { return x*x + y*y + z*z + w*w; } //! Computes magnitude inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z + w*w); } //! Normalize the vector inline_ HPoint& Normalize() { float M = Magnitude(); if(M) { M = 1.0f / M; x *= M; y *= M; z *= M; w *= M; } return *this; } // Arithmetic operators //! Operator for HPoint Negate = - HPoint; inline_ HPoint operator-() const { return HPoint(-x, -y, -z, -w); } //! Operator for HPoint Plus = HPoint + HPoint; inline_ HPoint operator+(const HPoint& p) const { return HPoint(x + p.x, y + p.y, z + p.z, w + p.w); } //! Operator for HPoint Minus = HPoint - HPoint; inline_ HPoint operator-(const HPoint& p) const { return HPoint(x - p.x, y - p.y, z - p.z, w - p.w); } //! Operator for HPoint Mul = HPoint * HPoint; inline_ HPoint operator*(const HPoint& p) const { return HPoint(x * p.x, y * p.y, z * p.z, w * p.w); } //! Operator for HPoint Scale = HPoint * float; inline_ HPoint operator*(float s) const { return HPoint(x * s, y * s, z * s, w * s); } //! Operator for HPoint Scale = float * HPoint; inline_ friend HPoint operator*(float s, const HPoint& p) { return HPoint(s * p.x, s * p.y, s * p.z, s * p.w); } //! Operator for HPoint Div = HPoint / HPoint; inline_ HPoint operator/(const HPoint& p) const { return HPoint(x / p.x, y / p.y, z / p.z, w / p.w); } //! Operator for HPoint Scale = HPoint / float; inline_ HPoint operator/(float s) const { s = 1.0f / s; return HPoint(x * s, y * s, z * s, w * s); } //! Operator for HPoint Scale = float / HPoint; inline_ friend HPoint operator/(float s, const HPoint& p) { return HPoint(s / p.x, s / p.y, s / p.z, s / p.w); } //! Operator for float DotProd = HPoint | HPoint; inline_ float operator|(const HPoint& p) const { return x*p.x + y*p.y + z*p.z + w*p.w; } // No cross-product in 4D //! Operator for HPoint += HPoint; inline_ HPoint& operator+=(const HPoint& p) { x += p.x; y += p.y; z += p.z; w += p.w; return *this; } //! Operator for HPoint += float; inline_ HPoint& operator+=(float s) { x += s; y += s; z += s; w += s; return *this; } //! Operator for HPoint -= HPoint; inline_ HPoint& operator-=(const HPoint& p) { x -= p.x; y -= p.y; z -= p.z; w -= p.w; return *this; } //! Operator for HPoint -= float; inline_ HPoint& operator-=(float s) { x -= s; y -= s; z -= s; w -= s; return *this; } //! Operator for HPoint *= HPoint; inline_ HPoint& operator*=(const HPoint& p) { x *= p.x; y *= p.y; z *= p.z; w *= p.w; return *this; } //! Operator for HPoint *= float; inline_ HPoint& operator*=(float s) { x*=s; y*=s; z*=s; w*=s; return *this; } //! Operator for HPoint /= HPoint; inline_ HPoint& operator/=(const HPoint& p) { x /= p.x; y /= p.y; z /= p.z; w /= p.w; return *this; } //! Operator for HPoint /= float; inline_ HPoint& operator/=(float s) { s = 1.0f / s; x*=s; y*=s; z*=s; w*=s; return *this; } // Arithmetic operators //! Operator for Point Mul = HPoint * Matrix3x3; Point operator*(const Matrix3x3& mat) const; //! Operator for HPoint Mul = HPoint * Matrix4x4; HPoint operator*(const Matrix4x4& mat) const; // HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4 //! Operator for HPoint *= Matrix4x4 HPoint& operator*=(const Matrix4x4& mat); // Logical operators //! Operator for "if(HPoint==HPoint)" inline_ bool operator==(const HPoint& p) const { return ( (x==p.x)&&(y==p.y)&&(z==p.z)&&(w==p.w)); } //! Operator for "if(HPoint!=HPoint)" inline_ bool operator!=(const HPoint& p) const { return ( (x!=p.x)||(y!=p.y)||(z!=p.z)||(w!=p.w)); } // Cast operators //! Cast a HPoint to a Point. w is discarded. #ifdef _MSC_VER inline_ operator Point() const { return Point(x, y, z); } // gcc complains that conversion to a base class will never use a type conversion operator #endif public: float w; }; #endif // __ICEHPOINT_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceIndexedTriangle.cpp000066400000000000000000000572151207742442300254260ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a handy indexed triangle class. * \file IceIndexedTriangle.cpp * \author Pierre Terdiman * \date January, 17, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains an indexed triangle class. * * \class Triangle * \author Pierre Terdiman * \version 1.0 * \date 08.15.98 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Flips the winding order. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void IndexedTriangle::Flip() { Swap(mVRef[1], mVRef[2]); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle area. * \param verts [in] the list of indexed vertices * \return the area */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float IndexedTriangle::Area(const Point* verts) const { if(!verts) return 0.0f; const Point& p0 = verts[0]; const Point& p1 = verts[1]; const Point& p2 = verts[2]; return ((p0-p1)^(p0-p2)).Magnitude() * 0.5f; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle perimeter. * \param verts [in] the list of indexed vertices * \return the perimeter */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float IndexedTriangle::Perimeter(const Point* verts) const { if(!verts) return 0.0f; const Point& p0 = verts[0]; const Point& p1 = verts[1]; const Point& p2 = verts[2]; return p0.Distance(p1) + p0.Distance(p2) + p1.Distance(p2); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle compacity. * \param verts [in] the list of indexed vertices * \return the compacity */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float IndexedTriangle::Compacity(const Point* verts) const { if(!verts) return 0.0f; float P = Perimeter(verts); if(P==0.0f) return 0.0f; return (4.0f*PI*Area(verts)/(P*P)); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle normal. * \param verts [in] the list of indexed vertices * \param normal [out] the computed normal */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void IndexedTriangle::Normal(const Point* verts, Point& normal) const { if(!verts) return; const Point& p0 = verts[mVRef[0]]; const Point& p1 = verts[mVRef[1]]; const Point& p2 = verts[mVRef[2]]; normal = ((p2-p1)^(p0-p1)).Normalize(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle denormalized normal. * \param verts [in] the list of indexed vertices * \param normal [out] the computed normal */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void IndexedTriangle::DenormalizedNormal(const Point* verts, Point& normal) const { if(!verts) return; const Point& p0 = verts[mVRef[0]]; const Point& p1 = verts[mVRef[1]]; const Point& p2 = verts[mVRef[2]]; normal = ((p2-p1)^(p0-p1)); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle center. * \param verts [in] the list of indexed vertices * \param center [out] the computed center */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void IndexedTriangle::Center(const Point* verts, Point& center) const { if(!verts) return; const Point& p0 = verts[mVRef[0]]; const Point& p1 = verts[mVRef[1]]; const Point& p2 = verts[mVRef[2]]; center = (p0+p1+p2)*INV3; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the centered normal * \param verts [in] the list of indexed vertices * \param normal [out] the computed centered normal */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void IndexedTriangle::CenteredNormal(const Point* verts, Point& normal) const { if(!verts) return; const Point& p0 = verts[mVRef[0]]; const Point& p1 = verts[mVRef[1]]; const Point& p2 = verts[mVRef[2]]; Point Center = (p0+p1+p2)*INV3; normal = Center + ((p2-p1)^(p0-p1)).Normalize(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes a random point within the triangle. * \param verts [in] the list of indexed vertices * \param normal [out] the computed centered normal */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void IndexedTriangle::RandomPoint(const Point* verts, Point& random) const { if(!verts) return; // Random barycentric coords float Alpha = UnitRandomFloat(); float Beta = UnitRandomFloat(); float Gamma = UnitRandomFloat(); float OneOverTotal = 1.0f / (Alpha + Beta + Gamma); Alpha *= OneOverTotal; Beta *= OneOverTotal; Gamma *= OneOverTotal; const Point& p0 = verts[mVRef[0]]; const Point& p1 = verts[mVRef[1]]; const Point& p2 = verts[mVRef[2]]; random = Alpha*p0 + Beta*p1 + Gamma*p2; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes backface culling. * \param verts [in] the list of indexed vertices * \param source [in] source point (in local space) from which culling must be computed * \return true if the triangle is visible from the source point */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool IndexedTriangle::IsVisible(const Point* verts, const Point& source) const { // Checkings if(!verts) return false; const Point& p0 = verts[mVRef[0]]; const Point& p1 = verts[mVRef[1]]; const Point& p2 = verts[mVRef[2]]; // Compute denormalized normal Point Normal = (p2 - p1)^(p0 - p1); // Backface culling return (Normal | source) >= 0.0f; // Same as: // Plane PL(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]); // return PL.Distance(source) > PL.d; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes backface culling. * \param verts [in] the list of indexed vertices * \param source [in] source point (in local space) from which culling must be computed * \return true if the triangle is visible from the source point */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool IndexedTriangle::BackfaceCulling(const Point* verts, const Point& source) const { // Checkings if(!verts) return false; const Point& p0 = verts[mVRef[0]]; const Point& p1 = verts[mVRef[1]]; const Point& p2 = verts[mVRef[2]]; // Compute base // Point Base = (p0 + p1 + p2)*INV3; // Compute denormalized normal Point Normal = (p2 - p1)^(p0 - p1); // Backface culling // return (Normal | (source - Base)) >= 0.0f; return (Normal | (source - p0)) >= 0.0f; // Same as: (but a bit faster) // Plane PL(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]); // return PL.Distance(source)>0.0f; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the occlusion potential of the triangle. * \param verts [in] the list of indexed vertices * \param source [in] source point (in local space) from which occlusion potential must be computed * \return the occlusion potential */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float IndexedTriangle::ComputeOcclusionPotential(const Point* verts, const Point& view) const { if(!verts) return 0.0f; // Occlusion potential: -(A * (N|V) / d^2) // A = polygon area // N = polygon normal // V = view vector // d = distance viewpoint-center of polygon float A = Area(verts); Point N; Normal(verts, N); Point C; Center(verts, C); float d = view.Distance(C); return -(A*(N|view))/(d*d); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Replaces a vertex reference with another one. * \param oldref [in] the vertex reference to replace * \param newref [in] the new vertex reference * \return true if success, else false if the input vertex reference doesn't belong to the triangle */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool IndexedTriangle::ReplaceVertex(udword oldref, udword newref) { if(mVRef[0]==oldref) { mVRef[0] = newref; return true; } else if(mVRef[1]==oldref) { mVRef[1] = newref; return true; } else if(mVRef[2]==oldref) { mVRef[2] = newref; return true; } return false; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks whether the triangle is degenerate or not. A degenerate triangle has two common vertex references. This is a zero-area triangle. * \return true if the triangle is degenerate */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool IndexedTriangle::IsDegenerate() const { if(mVRef[0]==mVRef[1]) return true; if(mVRef[1]==mVRef[2]) return true; if(mVRef[2]==mVRef[0]) return true; return false; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks whether the input vertex reference belongs to the triangle or not. * \param ref [in] the vertex reference to look for * \return true if the triangle contains the vertex reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool IndexedTriangle::HasVertex(udword ref) const { if(mVRef[0]==ref) return true; if(mVRef[1]==ref) return true; if(mVRef[2]==ref) return true; return false; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks whether the input vertex reference belongs to the triangle or not. * \param ref [in] the vertex reference to look for * \param index [out] the corresponding index in the triangle * \return true if the triangle contains the vertex reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool IndexedTriangle::HasVertex(udword ref, udword* index) const { if(mVRef[0]==ref) { *index = 0; return true; } if(mVRef[1]==ref) { *index = 1; return true; } if(mVRef[2]==ref) { *index = 2; return true; } return false; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Finds an edge in a tri, given two vertex references. * \param vref0 [in] the edge's first vertex reference * \param vref1 [in] the edge's second vertex reference * \return the edge number between 0 and 2, or 0xff if input refs are wrong. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ubyte IndexedTriangle::FindEdge(udword vref0, udword vref1) const { if(mVRef[0]==vref0 && mVRef[1]==vref1) return 0; else if(mVRef[0]==vref1 && mVRef[1]==vref0) return 0; else if(mVRef[0]==vref0 && mVRef[2]==vref1) return 1; else if(mVRef[0]==vref1 && mVRef[2]==vref0) return 1; else if(mVRef[1]==vref0 && mVRef[2]==vref1) return 2; else if(mVRef[1]==vref1 && mVRef[2]==vref0) return 2; return 0xff; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the last reference given the first two. * \param vref0 [in] the first vertex reference * \param vref1 [in] the second vertex reference * \return the last reference, or INVALID_ID if input refs are wrong. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword IndexedTriangle::OppositeVertex(udword vref0, udword vref1) const { if(mVRef[0]==vref0 && mVRef[1]==vref1) return mVRef[2]; else if(mVRef[0]==vref1 && mVRef[1]==vref0) return mVRef[2]; else if(mVRef[0]==vref0 && mVRef[2]==vref1) return mVRef[1]; else if(mVRef[0]==vref1 && mVRef[2]==vref0) return mVRef[1]; else if(mVRef[1]==vref0 && mVRef[2]==vref1) return mVRef[0]; else if(mVRef[1]==vref1 && mVRef[2]==vref0) return mVRef[0]; return INVALID_ID; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the three sorted vertex references according to an edge number. * edgenb = 0 => edge 0-1, returns references 0, 1, 2 * edgenb = 1 => edge 0-2, returns references 0, 2, 1 * edgenb = 2 => edge 1-2, returns references 1, 2, 0 * * \param edgenb [in] the edge number, 0, 1 or 2 * \param vref0 [out] the returned first vertex reference * \param vref1 [out] the returned second vertex reference * \param vref2 [out] the returned third vertex reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void IndexedTriangle::GetVRefs(ubyte edgenb, udword& vref0, udword& vref1, udword& vref2) const { if(edgenb==0) { vref0 = mVRef[0]; vref1 = mVRef[1]; vref2 = mVRef[2]; } else if(edgenb==1) { vref0 = mVRef[0]; vref1 = mVRef[2]; vref2 = mVRef[1]; } else if(edgenb==2) { vref0 = mVRef[1]; vref1 = mVRef[2]; vref2 = mVRef[0]; } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle's smallest edge length. * \param verts [in] the list of indexed vertices * \return the smallest edge length */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float IndexedTriangle::MinEdgeLength(const Point* verts) const { if(!verts) return 0.0f; float Min = MAX_FLOAT; float Length01 = verts[0].Distance(verts[1]); float Length02 = verts[0].Distance(verts[2]); float Length12 = verts[1].Distance(verts[2]); if(Length01 < Min) Min = Length01; if(Length02 < Min) Min = Length02; if(Length12 < Min) Min = Length12; return Min; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle's largest edge length. * \param verts [in] the list of indexed vertices * \return the largest edge length */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float IndexedTriangle::MaxEdgeLength(const Point* verts) const { if(!verts) return 0.0f; float Max = MIN_FLOAT; float Length01 = verts[0].Distance(verts[1]); float Length02 = verts[0].Distance(verts[2]); float Length12 = verts[1].Distance(verts[2]); if(Length01 > Max) Max = Length01; if(Length02 > Max) Max = Length02; if(Length12 > Max) Max = Length12; return Max; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes a point on the triangle according to the stabbing information. * \param verts [in] the list of indexed vertices * \param u,v [in] point's barycentric coordinates * \param pt [out] point on triangle * \param nearvtx [out] index of nearest vertex */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void IndexedTriangle::ComputePoint(const Point* verts, float u, float v, Point& pt, udword* nearvtx) const { // Checkings if(!verts) return; // Get face in local or global space const Point& p0 = verts[mVRef[0]]; const Point& p1 = verts[mVRef[1]]; const Point& p2 = verts[mVRef[2]]; // Compute point coordinates pt = (1.0f - u - v)*p0 + u*p1 + v*p2; // Compute nearest vertex if needed if(nearvtx) { // Compute distance vector Point d(p0.SquareDistance(pt), // Distance^2 from vertex 0 to point on the face p1.SquareDistance(pt), // Distance^2 from vertex 1 to point on the face p2.SquareDistance(pt)); // Distance^2 from vertex 2 to point on the face // Get smallest distance *nearvtx = mVRef[d.SmallestAxis()]; } } //************************************** // Angle between two vectors (in radians) // we use this formula // uv = |u||v| cos(u,v) // u ^ v = w // |w| = |u||v| |sin(u,v)| //************************************** float Angle(const Point& u, const Point& v) { float NormU = u.Magnitude(); // |u| float NormV = v.Magnitude(); // |v| float Product = NormU*NormV; // |u||v| if(Product==0.0f) return 0.0f; float OneOverProduct = 1.0f / Product; // Cosinus float Cosinus = (u|v) * OneOverProduct; // Sinus Point w = u^v; float NormW = w.Magnitude(); float AbsSinus = NormW * OneOverProduct; // Remove degeneracy if(AbsSinus > 1.0f) AbsSinus = 1.0f; if(AbsSinus < -1.0f) AbsSinus = -1.0f; if(Cosinus>=0.0f) return asinf(AbsSinus); else return (PI-asinf(AbsSinus)); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the angle between two triangles. * \param tri [in] the other triangle * \param verts [in] the list of indexed vertices * \return the angle in radians */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float IndexedTriangle::Angle(const IndexedTriangle& tri, const Point* verts) const { // Checkings if(!verts) return 0.0f; // Compute face normals Point n0, n1; Normal(verts, n0); tri.Normal(verts, n1); // Compute angle float dp = n0|n1; if(dp>1.0f) return 0.0f; if(dp<-1.0f) return PI; return acosf(dp); // return ::Angle(n0,n1); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks a triangle is the same as another one. * \param tri [in] the other triangle * \return true if same triangle */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool IndexedTriangle::Equal(const IndexedTriangle& tri) const { // Test all vertex references return (HasVertex(tri.mVRef[0]) && HasVertex(tri.mVRef[1]) && HasVertex(tri.mVRef[2])); } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceIndexedTriangle.h000066400000000000000000000065431207742442300250710ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a handy indexed triangle class. * \file IceIndexedTriangle.h * \author Pierre Terdiman * \date January, 17, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEINDEXEDTRIANGLE_H__ #define __ICEINDEXEDTRIANGLE_H__ // Forward declarations #ifdef _MSC_VER enum CubeIndex; #else typedef int CubeIndex; #endif // An indexed triangle class. class ICEMATHS_API IndexedTriangle { public: //! Constructor inline_ IndexedTriangle() {} //! Constructor inline_ IndexedTriangle(udword r0, udword r1, udword r2) { mVRef[0]=r0; mVRef[1]=r1; mVRef[2]=r2; } //! Copy constructor inline_ IndexedTriangle(const IndexedTriangle& triangle) { mVRef[0] = triangle.mVRef[0]; mVRef[1] = triangle.mVRef[1]; mVRef[2] = triangle.mVRef[2]; } //! Destructor inline_ ~IndexedTriangle() {} //! Vertex-references udword mVRef[3]; // Methods void Flip(); float Area(const Point* verts) const; float Perimeter(const Point* verts) const; float Compacity(const Point* verts) const; void Normal(const Point* verts, Point& normal) const; void DenormalizedNormal(const Point* verts, Point& normal) const; void Center(const Point* verts, Point& center) const; void CenteredNormal(const Point* verts, Point& normal) const; void RandomPoint(const Point* verts, Point& random) const; bool IsVisible(const Point* verts, const Point& source) const; bool BackfaceCulling(const Point* verts, const Point& source) const; float ComputeOcclusionPotential(const Point* verts, const Point& view) const; bool ReplaceVertex(udword oldref, udword newref); bool IsDegenerate() const; bool HasVertex(udword ref) const; bool HasVertex(udword ref, udword* index) const; ubyte FindEdge(udword vref0, udword vref1) const; udword OppositeVertex(udword vref0, udword vref1) const; inline_ udword OppositeVertex(ubyte edgenb) const { return mVRef[2-edgenb]; } void GetVRefs(ubyte edgenb, udword& vref0, udword& vref1, udword& vref2) const; float MinEdgeLength(const Point* verts) const; float MaxEdgeLength(const Point* verts) const; void ComputePoint(const Point* verts, float u, float v, Point& pt, udword* nearvtx=null) const; float Angle(const IndexedTriangle& tri, const Point* verts) const; inline_ Plane PlaneEquation(const Point* verts) const { return Plane(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]); } bool Equal(const IndexedTriangle& tri) const; CubeIndex ComputeCubeIndex(const Point* verts) const; }; #endif // __ICEINDEXEDTRIANGLE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceLSS.h000066400000000000000000000075611207742442300224650ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for line-swept spheres. * \file IceLSS.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICELSS_H__ #define __ICELSS_H__ class ICEMATHS_API LSS : public Segment { public: //! Constructor inline_ LSS() {} //! Constructor inline_ LSS(const Segment& seg, float radius) : Segment(seg), mRadius(radius) {} //! Destructor inline_ ~LSS() {} /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes an OBB surrounding the LSS. * \param box [out] the OBB */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void ComputeOBB(OBB& box); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if a point is contained within the LSS. * \param pt [in] the point to test * \return true if inside the LSS * \warning point and LSS must be in same space */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ bool Contains(const Point& pt) const { return SquareDistance(pt) <= mRadius*mRadius; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if a sphere is contained within the LSS. * \param sphere [in] the sphere to test * \return true if inside the LSS * \warning sphere and LSS must be in same space */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ bool Contains(const Sphere& sphere) { float d = mRadius - sphere.mRadius; if(d>=0.0f) return SquareDistance(sphere.mCenter) <= d*d; else return false; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if an LSS is contained within the LSS. * \param lss [in] the LSS to test * \return true if inside the LSS * \warning both LSS must be in same space */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ bool Contains(const LSS& lss) { // We check the LSS contains the two spheres at the start and end of the sweep return Contains(Sphere(lss.mP0, lss.mRadius)) && Contains(Sphere(lss.mP0, lss.mRadius)); } float mRadius; //!< Sphere radius }; #endif // __ICELSS_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceMatrix3x3.cpp000066400000000000000000000034631207742442300241560ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for 3x3 matrices. * \file IceMatrix3x3.cpp * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * 3x3 matrix. * DirectX-compliant, ie row-column order, ie m[Row][Col]. * Same as: * m11 m12 m13 first row. * m21 m22 m23 second row. * m31 m32 m33 third row. * Stored in memory as m11 m12 m13 m21... * * Multiplication rules: * * [x'y'z'] = [xyz][M] * * x' = x*m11 + y*m21 + z*m31 * y' = x*m12 + y*m22 + z*m32 * z' = x*m13 + y*m23 + z*m33 * * \class Matrix3x3 * \author Pierre Terdiman * \version 1.0 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; // Cast operator Matrix3x3::operator Matrix4x4() const { return Matrix4x4( m[0][0], m[0][1], m[0][2], 0.0f, m[1][0], m[1][1], m[1][2], 0.0f, m[2][0], m[2][1], m[2][2], 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceMatrix3x3.h000066400000000000000000000500251207742442300236170ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for 3x3 matrices. * \file IceMatrix3x3.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEMATRIX3X3_H__ #define __ICEMATRIX3X3_H__ // Forward declarations class Quat; #define MATRIX3X3_EPSILON (1.0e-7f) class ICEMATHS_API Matrix3x3 { public: //! Empty constructor inline_ Matrix3x3() {} //! Constructor from 9 values inline_ Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) { m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; } //! Copy constructor inline_ Matrix3x3(const Matrix3x3& mat) { CopyMemory(m, &mat.m, 9*sizeof(float)); } //! Destructor inline_ ~Matrix3x3() {} //! Assign values inline_ void Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) { m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; } //! Sets the scale from a Point. The point is put on the diagonal. inline_ void SetScale(const Point& p) { m[0][0] = p.x; m[1][1] = p.y; m[2][2] = p.z; } //! Sets the scale from floats. Values are put on the diagonal. inline_ void SetScale(float sx, float sy, float sz) { m[0][0] = sx; m[1][1] = sy; m[2][2] = sz; } //! Scales from a Point. Each row is multiplied by a component. inline_ void Scale(const Point& p) { m[0][0] *= p.x; m[0][1] *= p.x; m[0][2] *= p.x; m[1][0] *= p.y; m[1][1] *= p.y; m[1][2] *= p.y; m[2][0] *= p.z; m[2][1] *= p.z; m[2][2] *= p.z; } //! Scales from floats. Each row is multiplied by a value. inline_ void Scale(float sx, float sy, float sz) { m[0][0] *= sx; m[0][1] *= sx; m[0][2] *= sx; m[1][0] *= sy; m[1][1] *= sy; m[1][2] *= sy; m[2][0] *= sz; m[2][1] *= sz; m[2][2] *= sz; } //! Copy from a Matrix3x3 inline_ void Copy(const Matrix3x3& source) { CopyMemory(m, source.m, 9*sizeof(float)); } // Row-column access //! Returns a row. inline_ void GetRow(const udword r, Point& p) const { p.x = m[r][0]; p.y = m[r][1]; p.z = m[r][2]; } //! Returns a row. inline_ const Point& GetRow(const udword r) const { return *(const Point*)&m[r][0]; } //! Returns a row. inline_ Point& GetRow(const udword r) { return *(Point*)&m[r][0]; } //! Sets a row. inline_ void SetRow(const udword r, const Point& p) { m[r][0] = p.x; m[r][1] = p.y; m[r][2] = p.z; } //! Returns a column. inline_ void GetCol(const udword c, Point& p) const { p.x = m[0][c]; p.y = m[1][c]; p.z = m[2][c]; } //! Sets a column. inline_ void SetCol(const udword c, const Point& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; } //! Computes the trace. The trace is the sum of the 3 diagonal components. inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2]; } //! Clears the matrix. inline_ void Zero() { ZeroMemory(&m, sizeof(m)); } //! Sets the identity matrix. inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f; } //! Checks for identity inline_ bool IsIdentity() const { if(IR(m[0][0])!=IEEE_1_0) return false; if(IR(m[0][1])!=0) return false; if(IR(m[0][2])!=0) return false; if(IR(m[1][0])!=0) return false; if(IR(m[1][1])!=IEEE_1_0) return false; if(IR(m[1][2])!=0) return false; if(IR(m[2][0])!=0) return false; if(IR(m[2][1])!=0) return false; if(IR(m[2][2])!=IEEE_1_0) return false; return true; } //! Checks matrix validity inline_ BOOL IsValid() const { for(udword j=0;j<3;j++) { for(udword i=0;i<3;i++) { if(!IsValidFloat(m[j][i])) return FALSE; } } return TRUE; } //! Makes a skew-symmetric matrix (a.k.a. Star(*) Matrix) //! [ 0.0 -a.z a.y ] //! [ a.z 0.0 -a.x ] //! [ -a.y a.x 0.0 ] //! This is also called a "cross matrix" since for any vectors A and B, //! A^B = Skew(A) * B = - B * Skew(A); inline_ void SkewSymmetric(const Point& a) { m[0][0] = 0.0f; m[0][1] = -a.z; m[0][2] = a.y; m[1][0] = a.z; m[1][1] = 0.0f; m[1][2] = -a.x; m[2][0] = -a.y; m[2][1] = a.x; m[2][2] = 0.0f; } //! Negates the matrix inline_ void Neg() { m[0][0] = -m[0][0]; m[0][1] = -m[0][1]; m[0][2] = -m[0][2]; m[1][0] = -m[1][0]; m[1][1] = -m[1][1]; m[1][2] = -m[1][2]; m[2][0] = -m[2][0]; m[2][1] = -m[2][1]; m[2][2] = -m[2][2]; } //! Neg from another matrix inline_ void Neg(const Matrix3x3& mat) { m[0][0] = -mat.m[0][0]; m[0][1] = -mat.m[0][1]; m[0][2] = -mat.m[0][2]; m[1][0] = -mat.m[1][0]; m[1][1] = -mat.m[1][1]; m[1][2] = -mat.m[1][2]; m[2][0] = -mat.m[2][0]; m[2][1] = -mat.m[2][1]; m[2][2] = -mat.m[2][2]; } //! Add another matrix inline_ void Add(const Matrix3x3& mat) { m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2]; m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2]; m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2]; } //! Sub another matrix inline_ void Sub(const Matrix3x3& mat) { m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2]; m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2]; m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2]; } //! Mac inline_ void Mac(const Matrix3x3& a, const Matrix3x3& b, float s) { m[0][0] = a.m[0][0] + b.m[0][0] * s; m[0][1] = a.m[0][1] + b.m[0][1] * s; m[0][2] = a.m[0][2] + b.m[0][2] * s; m[1][0] = a.m[1][0] + b.m[1][0] * s; m[1][1] = a.m[1][1] + b.m[1][1] * s; m[1][2] = a.m[1][2] + b.m[1][2] * s; m[2][0] = a.m[2][0] + b.m[2][0] * s; m[2][1] = a.m[2][1] + b.m[2][1] * s; m[2][2] = a.m[2][2] + b.m[2][2] * s; } //! Mac inline_ void Mac(const Matrix3x3& a, float s) { m[0][0] += a.m[0][0] * s; m[0][1] += a.m[0][1] * s; m[0][2] += a.m[0][2] * s; m[1][0] += a.m[1][0] * s; m[1][1] += a.m[1][1] * s; m[1][2] += a.m[1][2] * s; m[2][0] += a.m[2][0] * s; m[2][1] += a.m[2][1] * s; m[2][2] += a.m[2][2] * s; } //! this = A * s inline_ void Mult(const Matrix3x3& a, float s) { m[0][0] = a.m[0][0] * s; m[0][1] = a.m[0][1] * s; m[0][2] = a.m[0][2] * s; m[1][0] = a.m[1][0] * s; m[1][1] = a.m[1][1] * s; m[1][2] = a.m[1][2] * s; m[2][0] = a.m[2][0] * s; m[2][1] = a.m[2][1] * s; m[2][2] = a.m[2][2] * s; } inline_ void Add(const Matrix3x3& a, const Matrix3x3& b) { m[0][0] = a.m[0][0] + b.m[0][0]; m[0][1] = a.m[0][1] + b.m[0][1]; m[0][2] = a.m[0][2] + b.m[0][2]; m[1][0] = a.m[1][0] + b.m[1][0]; m[1][1] = a.m[1][1] + b.m[1][1]; m[1][2] = a.m[1][2] + b.m[1][2]; m[2][0] = a.m[2][0] + b.m[2][0]; m[2][1] = a.m[2][1] + b.m[2][1]; m[2][2] = a.m[2][2] + b.m[2][2]; } inline_ void Sub(const Matrix3x3& a, const Matrix3x3& b) { m[0][0] = a.m[0][0] - b.m[0][0]; m[0][1] = a.m[0][1] - b.m[0][1]; m[0][2] = a.m[0][2] - b.m[0][2]; m[1][0] = a.m[1][0] - b.m[1][0]; m[1][1] = a.m[1][1] - b.m[1][1]; m[1][2] = a.m[1][2] - b.m[1][2]; m[2][0] = a.m[2][0] - b.m[2][0]; m[2][1] = a.m[2][1] - b.m[2][1]; m[2][2] = a.m[2][2] - b.m[2][2]; } //! this = a * b inline_ void Mult(const Matrix3x3& a, const Matrix3x3& b) { m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0]; m[0][1] = a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1]; m[0][2] = a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2]; m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0]; m[1][1] = a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1]; m[1][2] = a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2]; m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0]; m[2][1] = a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1]; m[2][2] = a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2]; } //! this = transpose(a) * b inline_ void MultAtB(const Matrix3x3& a, const Matrix3x3& b) { m[0][0] = a.m[0][0] * b.m[0][0] + a.m[1][0] * b.m[1][0] + a.m[2][0] * b.m[2][0]; m[0][1] = a.m[0][0] * b.m[0][1] + a.m[1][0] * b.m[1][1] + a.m[2][0] * b.m[2][1]; m[0][2] = a.m[0][0] * b.m[0][2] + a.m[1][0] * b.m[1][2] + a.m[2][0] * b.m[2][2]; m[1][0] = a.m[0][1] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[2][1] * b.m[2][0]; m[1][1] = a.m[0][1] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[2][1] * b.m[2][1]; m[1][2] = a.m[0][1] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[2][1] * b.m[2][2]; m[2][0] = a.m[0][2] * b.m[0][0] + a.m[1][2] * b.m[1][0] + a.m[2][2] * b.m[2][0]; m[2][1] = a.m[0][2] * b.m[0][1] + a.m[1][2] * b.m[1][1] + a.m[2][2] * b.m[2][1]; m[2][2] = a.m[0][2] * b.m[0][2] + a.m[1][2] * b.m[1][2] + a.m[2][2] * b.m[2][2]; } //! this = a * transpose(b) inline_ void MultABt(const Matrix3x3& a, const Matrix3x3& b) { m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[0][1] + a.m[0][2] * b.m[0][2]; m[0][1] = a.m[0][0] * b.m[1][0] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[1][2]; m[0][2] = a.m[0][0] * b.m[2][0] + a.m[0][1] * b.m[2][1] + a.m[0][2] * b.m[2][2]; m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[0][1] + a.m[1][2] * b.m[0][2]; m[1][1] = a.m[1][0] * b.m[1][0] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[1][2]; m[1][2] = a.m[1][0] * b.m[2][0] + a.m[1][1] * b.m[2][1] + a.m[1][2] * b.m[2][2]; m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[0][1] + a.m[2][2] * b.m[0][2]; m[2][1] = a.m[2][0] * b.m[1][0] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[1][2]; m[2][2] = a.m[2][0] * b.m[2][0] + a.m[2][1] * b.m[2][1] + a.m[2][2] * b.m[2][2]; } //! Makes a rotation matrix mapping vector "from" to vector "to". Matrix3x3& FromTo(const Point& from, const Point& to); //! Set a rotation matrix around the X axis. //! 1 0 0 //! RX = 0 cx sx //! 0 -sx cx void RotX(float angle); //! Set a rotation matrix around the Y axis. //! cy 0 -sy //! RY = 0 1 0 //! sy 0 cy void RotY(float angle); //! Set a rotation matrix around the Z axis. //! cz sz 0 //! RZ = -sz cz 0 //! 0 0 1 void RotZ(float angle); //! cy sx.sy -sy.cx //! RY.RX 0 cx sx //! sy -sx.cy cx.cy void RotYX(float y, float x); //! Make a rotation matrix about an arbitrary axis Matrix3x3& Rot(float angle, const Point& axis); //! Transpose the matrix. void Transpose() { IR(m[1][0]) ^= IR(m[0][1]); IR(m[0][1]) ^= IR(m[1][0]); IR(m[1][0]) ^= IR(m[0][1]); IR(m[2][0]) ^= IR(m[0][2]); IR(m[0][2]) ^= IR(m[2][0]); IR(m[2][0]) ^= IR(m[0][2]); IR(m[2][1]) ^= IR(m[1][2]); IR(m[1][2]) ^= IR(m[2][1]); IR(m[2][1]) ^= IR(m[1][2]); } //! this = Transpose(a) void Transpose(const Matrix3x3& a) { m[0][0] = a.m[0][0]; m[0][1] = a.m[1][0]; m[0][2] = a.m[2][0]; m[1][0] = a.m[0][1]; m[1][1] = a.m[1][1]; m[1][2] = a.m[2][1]; m[2][0] = a.m[0][2]; m[2][1] = a.m[1][2]; m[2][2] = a.m[2][2]; } //! Compute the determinant of the matrix. We use the rule of Sarrus. float Determinant() const { return (m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1]) - (m[2][0]*m[1][1]*m[0][2] + m[2][1]*m[1][2]*m[0][0] + m[2][2]*m[1][0]*m[0][1]); } /* //! Compute a cofactor. Used for matrix inversion. float CoFactor(ubyte row, ubyte column) const { static sdword gIndex[3+2] = { 0, 1, 2, 0, 1 }; return (m[gIndex[row+1]][gIndex[column+1]]*m[gIndex[row+2]][gIndex[column+2]] - m[gIndex[row+2]][gIndex[column+1]]*m[gIndex[row+1]][gIndex[column+2]]); } */ //! Invert the matrix. Determinant must be different from zero, else matrix can't be inverted. Matrix3x3& Invert() { float Det = Determinant(); // Must be !=0 float OneOverDet = 1.0f / Det; Matrix3x3 Temp; Temp.m[0][0] = +(m[1][1] * m[2][2] - m[2][1] * m[1][2]) * OneOverDet; Temp.m[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * OneOverDet; Temp.m[2][0] = +(m[1][0] * m[2][1] - m[2][0] * m[1][1]) * OneOverDet; Temp.m[0][1] = -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * OneOverDet; Temp.m[1][1] = +(m[0][0] * m[2][2] - m[2][0] * m[0][2]) * OneOverDet; Temp.m[2][1] = -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * OneOverDet; Temp.m[0][2] = +(m[0][1] * m[1][2] - m[1][1] * m[0][2]) * OneOverDet; Temp.m[1][2] = -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * OneOverDet; Temp.m[2][2] = +(m[0][0] * m[1][1] - m[1][0] * m[0][1]) * OneOverDet; *this = Temp; return *this; } Matrix3x3& Normalize(); //! this = exp(a) Matrix3x3& Exp(const Matrix3x3& a); void FromQuat(const Quat &q); void FromQuatL2(const Quat &q, float l2); // Arithmetic operators //! Operator for Matrix3x3 Plus = Matrix3x3 + Matrix3x3; inline_ Matrix3x3 operator+(const Matrix3x3& mat) const { return Matrix3x3( m[0][0] + mat.m[0][0], m[0][1] + mat.m[0][1], m[0][2] + mat.m[0][2], m[1][0] + mat.m[1][0], m[1][1] + mat.m[1][1], m[1][2] + mat.m[1][2], m[2][0] + mat.m[2][0], m[2][1] + mat.m[2][1], m[2][2] + mat.m[2][2]); } //! Operator for Matrix3x3 Minus = Matrix3x3 - Matrix3x3; inline_ Matrix3x3 operator-(const Matrix3x3& mat) const { return Matrix3x3( m[0][0] - mat.m[0][0], m[0][1] - mat.m[0][1], m[0][2] - mat.m[0][2], m[1][0] - mat.m[1][0], m[1][1] - mat.m[1][1], m[1][2] - mat.m[1][2], m[2][0] - mat.m[2][0], m[2][1] - mat.m[2][1], m[2][2] - mat.m[2][2]); } //! Operator for Matrix3x3 Mul = Matrix3x3 * Matrix3x3; inline_ Matrix3x3 operator*(const Matrix3x3& mat) const { return Matrix3x3( m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0], m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1], m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2], m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0], m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1], m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2], m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0], m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1], m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2]); } //! Operator for Point Mul = Matrix3x3 * Point; inline_ Point operator*(const Point& v) const { return Point(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v); } //! Operator for Matrix3x3 Mul = Matrix3x3 * float; inline_ Matrix3x3 operator*(float s) const { return Matrix3x3( m[0][0]*s, m[0][1]*s, m[0][2]*s, m[1][0]*s, m[1][1]*s, m[1][2]*s, m[2][0]*s, m[2][1]*s, m[2][2]*s); } //! Operator for Matrix3x3 Mul = float * Matrix3x3; inline_ friend Matrix3x3 operator*(float s, const Matrix3x3& mat) { return Matrix3x3( s*mat.m[0][0], s*mat.m[0][1], s*mat.m[0][2], s*mat.m[1][0], s*mat.m[1][1], s*mat.m[1][2], s*mat.m[2][0], s*mat.m[2][1], s*mat.m[2][2]); } //! Operator for Matrix3x3 Div = Matrix3x3 / float; inline_ Matrix3x3 operator/(float s) const { if (s) s = 1.0f / s; return Matrix3x3( m[0][0]*s, m[0][1]*s, m[0][2]*s, m[1][0]*s, m[1][1]*s, m[1][2]*s, m[2][0]*s, m[2][1]*s, m[2][2]*s); } //! Operator for Matrix3x3 Div = float / Matrix3x3; inline_ friend Matrix3x3 operator/(float s, const Matrix3x3& mat) { return Matrix3x3( s/mat.m[0][0], s/mat.m[0][1], s/mat.m[0][2], s/mat.m[1][0], s/mat.m[1][1], s/mat.m[1][2], s/mat.m[2][0], s/mat.m[2][1], s/mat.m[2][2]); } //! Operator for Matrix3x3 += Matrix3x3 inline_ Matrix3x3& operator+=(const Matrix3x3& mat) { m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2]; m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2]; m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2]; return *this; } //! Operator for Matrix3x3 -= Matrix3x3 inline_ Matrix3x3& operator-=(const Matrix3x3& mat) { m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2]; m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2]; m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2]; return *this; } //! Operator for Matrix3x3 *= Matrix3x3 inline_ Matrix3x3& operator*=(const Matrix3x3& mat) { Point TempRow; GetRow(0, TempRow); m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; GetRow(1, TempRow); m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; GetRow(2, TempRow); m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; return *this; } //! Operator for Matrix3x3 *= float inline_ Matrix3x3& operator*=(float s) { m[0][0] *= s; m[0][1] *= s; m[0][2] *= s; m[1][0] *= s; m[1][1] *= s; m[1][2] *= s; m[2][0] *= s; m[2][1] *= s; m[2][2] *= s; return *this; } //! Operator for Matrix3x3 /= float inline_ Matrix3x3& operator/=(float s) { if (s) s = 1.0f / s; m[0][0] *= s; m[0][1] *= s; m[0][2] *= s; m[1][0] *= s; m[1][1] *= s; m[1][2] *= s; m[2][0] *= s; m[2][1] *= s; m[2][2] *= s; return *this; } // Cast operators //! Cast a Matrix3x3 to a Matrix4x4. operator Matrix4x4() const; //! Cast a Matrix3x3 to a Quat. operator Quat() const; inline_ const Point& operator[](int row) const { return *(const Point*)&m[row][0]; } inline_ Point& operator[](int row) { return *(Point*)&m[row][0]; } public: float m[3][3]; }; #endif // __ICEMATRIX3X3_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceMatrix4x4.cpp000066400000000000000000000140711207742442300241550ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for 4x4 matrices. * \file IceMatrix4x4.cpp * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * 4x4 matrix. * DirectX-compliant, ie row-column order, ie m[Row][Col]. * Same as: * m11 m12 m13 m14 first row. * m21 m22 m23 m24 second row. * m31 m32 m33 m34 third row. * m41 m42 m43 m44 fourth row. * Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1). * Stored in memory as m11 m12 m13 m14 m21... * * Multiplication rules: * * [x'y'z'1] = [xyz1][M] * * x' = x*m11 + y*m21 + z*m31 + m41 * y' = x*m12 + y*m22 + z*m32 + m42 * z' = x*m13 + y*m23 + z*m33 + m43 * 1' = 0 + 0 + 0 + m44 * * \class Matrix4x4 * \author Pierre Terdiman * \version 1.0 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Inverts a PR matrix. (which only contains a rotation and a translation) * This is faster and less subject to FPU errors than the generic inversion code. * * \relates Matrix4x4 * \fn InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src) * \param dest [out] destination matrix * \param src [in] source matrix */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src) { dest.m[0][0] = src.m[0][0]; dest.m[1][0] = src.m[0][1]; dest.m[2][0] = src.m[0][2]; dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]); dest.m[0][1] = src.m[1][0]; dest.m[1][1] = src.m[1][1]; dest.m[2][1] = src.m[1][2]; dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]); dest.m[0][2] = src.m[2][0]; dest.m[1][2] = src.m[2][1]; dest.m[2][2] = src.m[2][2]; dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]); dest.m[0][3] = 0.0f; dest.m[1][3] = 0.0f; dest.m[2][3] = 0.0f; dest.m[3][3] = 1.0f; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Compute the cofactor of the Matrix at a specified location /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float Matrix4x4::CoFactor(udword row, udword col) const { return (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] + m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] + m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3]) - (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] + m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] + m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Compute the determinant of the Matrix /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float Matrix4x4::Determinant() const { return m[0][0] * CoFactor(0, 0) + m[0][1] * CoFactor(0, 1) + m[0][2] * CoFactor(0, 2) + m[0][3] * CoFactor(0, 3); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Compute the inverse of the matrix /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Matrix4x4& Matrix4x4::Invert() { float Det = Determinant(); Matrix4x4 Temp; if(fabsf(Det) < MATRIX4X4_EPSILON) return *this; // The matrix is not invertible! Singular case! float IDet = 1.0f / Det; Temp.m[0][0] = CoFactor(0,0) * IDet; Temp.m[1][0] = CoFactor(0,1) * IDet; Temp.m[2][0] = CoFactor(0,2) * IDet; Temp.m[3][0] = CoFactor(0,3) * IDet; Temp.m[0][1] = CoFactor(1,0) * IDet; Temp.m[1][1] = CoFactor(1,1) * IDet; Temp.m[2][1] = CoFactor(1,2) * IDet; Temp.m[3][1] = CoFactor(1,3) * IDet; Temp.m[0][2] = CoFactor(2,0) * IDet; Temp.m[1][2] = CoFactor(2,1) * IDet; Temp.m[2][2] = CoFactor(2,2) * IDet; Temp.m[3][2] = CoFactor(2,3) * IDet; Temp.m[0][3] = CoFactor(3,0) * IDet; Temp.m[1][3] = CoFactor(3,1) * IDet; Temp.m[2][3] = CoFactor(3,2) * IDet; Temp.m[3][3] = CoFactor(3,3) * IDet; *this = Temp; return *this; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceMatrix4x4.h000066400000000000000000000511451207742442300236250ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for 4x4 matrices. * \file IceMatrix4x4.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEMATRIX4X4_H__ #define __ICEMATRIX4X4_H__ // Forward declarations class PRS; class PR; #define MATRIX4X4_EPSILON (1.0e-7f) class ICEMATHS_API Matrix4x4 { // void LUBackwardSubstitution( sdword *indx, float* b ); // void LUDecomposition( sdword* indx, float* d ); public: //! Empty constructor. inline_ Matrix4x4() {} //! Constructor from 16 values inline_ Matrix4x4( float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) { m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[0][3] = m03; m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[1][3] = m13; m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; m[2][3] = m23; m[3][0] = m30; m[3][1] = m31; m[3][2] = m32; m[3][3] = m33; } //! Copy constructor inline_ Matrix4x4(const Matrix4x4& mat) { CopyMemory(m, &mat.m, 16*sizeof(float)); } //! Destructor. inline_ ~Matrix4x4() {} //! Assign values (rotation only) inline_ Matrix4x4& Set( float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) { m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; return *this; } //! Assign values inline_ Matrix4x4& Set( float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) { m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[0][3] = m03; m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[1][3] = m13; m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; m[2][3] = m23; m[3][0] = m30; m[3][1] = m31; m[3][2] = m32; m[3][3] = m33; return *this; } //! Copy from a Matrix4x4 inline_ void Copy(const Matrix4x4& source) { CopyMemory(m, source.m, 16*sizeof(float)); } // Row-column access //! Returns a row. inline_ void GetRow(const udword r, HPoint& p) const { p.x=m[r][0]; p.y=m[r][1]; p.z=m[r][2]; p.w=m[r][3]; } //! Returns a row. inline_ void GetRow(const udword r, Point& p) const { p.x=m[r][0]; p.y=m[r][1]; p.z=m[r][2]; } //! Returns a row. inline_ const HPoint& GetRow(const udword r) const { return *(const HPoint*)&m[r][0]; } //! Returns a row. inline_ HPoint& GetRow(const udword r) { return *(HPoint*)&m[r][0]; } //! Sets a row. inline_ void SetRow(const udword r, const HPoint& p) { m[r][0]=p.x; m[r][1]=p.y; m[r][2]=p.z; m[r][3]=p.w; } //! Sets a row. inline_ void SetRow(const udword r, const Point& p) { m[r][0]=p.x; m[r][1]=p.y; m[r][2]=p.z; m[r][3]= (r!=3) ? 0.0f : 1.0f; } //! Returns a column. inline_ void GetCol(const udword c, HPoint& p) const { p.x=m[0][c]; p.y=m[1][c]; p.z=m[2][c]; p.w=m[3][c]; } //! Returns a column. inline_ void GetCol(const udword c, Point& p) const { p.x=m[0][c]; p.y=m[1][c]; p.z=m[2][c]; } //! Sets a column. inline_ void SetCol(const udword c, const HPoint& p) { m[0][c]=p.x; m[1][c]=p.y; m[2][c]=p.z; m[3][c]=p.w; } //! Sets a column. inline_ void SetCol(const udword c, const Point& p) { m[0][c]=p.x; m[1][c]=p.y; m[2][c]=p.z; m[3][c]= (c!=3) ? 0.0f : 1.0f; } // Translation //! Returns the translation part of the matrix. inline_ const HPoint& GetTrans() const { return GetRow(3); } //! Gets the translation part of the matrix inline_ void GetTrans(Point& p) const { p.x=m[3][0]; p.y=m[3][1]; p.z=m[3][2]; } //! Sets the translation part of the matrix, from a Point. inline_ void SetTrans(const Point& p) { m[3][0]=p.x; m[3][1]=p.y; m[3][2]=p.z; } //! Sets the translation part of the matrix, from a HPoint. inline_ void SetTrans(const HPoint& p) { m[3][0]=p.x; m[3][1]=p.y; m[3][2]=p.z; m[3][3]=p.w; } //! Sets the translation part of the matrix, from floats. inline_ void SetTrans(float tx, float ty, float tz) { m[3][0]=tx; m[3][1]=ty; m[3][2]=tz; } // Scale //! Sets the scale from a Point. The point is put on the diagonal. inline_ void SetScale(const Point& p) { m[0][0]=p.x; m[1][1]=p.y; m[2][2]=p.z; } //! Sets the scale from floats. Values are put on the diagonal. inline_ void SetScale(float sx, float sy, float sz) { m[0][0]=sx; m[1][1]=sy; m[2][2]=sz; } //! Scales from a Point. Each row is multiplied by a component. void Scale(const Point& p) { m[0][0] *= p.x; m[1][0] *= p.y; m[2][0] *= p.z; m[0][1] *= p.x; m[1][1] *= p.y; m[2][1] *= p.z; m[0][2] *= p.x; m[1][2] *= p.y; m[2][2] *= p.z; } //! Scales from floats. Each row is multiplied by a value. void Scale(float sx, float sy, float sz) { m[0][0] *= sx; m[1][0] *= sy; m[2][0] *= sz; m[0][1] *= sx; m[1][1] *= sy; m[2][1] *= sz; m[0][2] *= sx; m[1][2] *= sy; m[2][2] *= sz; } /* //! Returns a row. inline_ HPoint GetRow(const udword row) const { return mRow[row]; } //! Sets a row. inline_ Matrix4x4& SetRow(const udword row, const HPoint& p) { mRow[row] = p; return *this; } //! Sets a row. Matrix4x4& SetRow(const udword row, const Point& p) { m[row][0] = p.x; m[row][1] = p.y; m[row][2] = p.z; m[row][3] = (row != 3) ? 0.0f : 1.0f; return *this; } //! Returns a column. HPoint GetCol(const udword col) const { HPoint Res; Res.x = m[0][col]; Res.y = m[1][col]; Res.z = m[2][col]; Res.w = m[3][col]; return Res; } //! Sets a column. Matrix4x4& SetCol(const udword col, const HPoint& p) { m[0][col] = p.x; m[1][col] = p.y; m[2][col] = p.z; m[3][col] = p.w; return *this; } //! Sets a column. Matrix4x4& SetCol(const udword col, const Point& p) { m[0][col] = p.x; m[1][col] = p.y; m[2][col] = p.z; m[3][col] = (col != 3) ? 0.0f : 1.0f; return *this; } */ //! Computes the trace. The trace is the sum of the 4 diagonal components. inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2] + m[3][3]; } //! Computes the trace of the upper 3x3 matrix. inline_ float Trace3x3() const { return m[0][0] + m[1][1] + m[2][2]; } //! Clears the matrix. inline_ void Zero() { ZeroMemory(&m, sizeof(m)); } //! Sets the identity matrix. inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f; } //! Checks for identity inline_ bool IsIdentity() const { if(IR(m[0][0])!=IEEE_1_0) return false; if(IR(m[0][1])!=0) return false; if(IR(m[0][2])!=0) return false; if(IR(m[0][3])!=0) return false; if(IR(m[1][0])!=0) return false; if(IR(m[1][1])!=IEEE_1_0) return false; if(IR(m[1][2])!=0) return false; if(IR(m[1][3])!=0) return false; if(IR(m[2][0])!=0) return false; if(IR(m[2][1])!=0) return false; if(IR(m[2][2])!=IEEE_1_0) return false; if(IR(m[2][3])!=0) return false; if(IR(m[3][0])!=0) return false; if(IR(m[3][1])!=0) return false; if(IR(m[3][2])!=0) return false; if(IR(m[3][3])!=IEEE_1_0) return false; return true; } //! Checks matrix validity inline_ BOOL IsValid() const { for(udword j=0;j<4;j++) { for(udword i=0;i<4;i++) { if(!IsValidFloat(m[j][i])) return FALSE; } } return TRUE; } //! Sets a rotation matrix around the X axis. void RotX(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[1][1] = m[2][2] = Cos; m[2][1] = -Sin; m[1][2] = Sin; } //! Sets a rotation matrix around the Y axis. void RotY(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[2][2] = Cos; m[2][0] = Sin; m[0][2] = -Sin; } //! Sets a rotation matrix around the Z axis. void RotZ(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[1][1] = Cos; m[1][0] = -Sin; m[0][1] = Sin; } //! Makes a rotation matrix about an arbitrary axis Matrix4x4& Rot(float angle, Point& p1, Point& p2); //! Transposes the matrix. void Transpose() { IR(m[1][0]) ^= IR(m[0][1]); IR(m[0][1]) ^= IR(m[1][0]); IR(m[1][0]) ^= IR(m[0][1]); IR(m[2][0]) ^= IR(m[0][2]); IR(m[0][2]) ^= IR(m[2][0]); IR(m[2][0]) ^= IR(m[0][2]); IR(m[3][0]) ^= IR(m[0][3]); IR(m[0][3]) ^= IR(m[3][0]); IR(m[3][0]) ^= IR(m[0][3]); IR(m[1][2]) ^= IR(m[2][1]); IR(m[2][1]) ^= IR(m[1][2]); IR(m[1][2]) ^= IR(m[2][1]); IR(m[1][3]) ^= IR(m[3][1]); IR(m[3][1]) ^= IR(m[1][3]); IR(m[1][3]) ^= IR(m[3][1]); IR(m[2][3]) ^= IR(m[3][2]); IR(m[3][2]) ^= IR(m[2][3]); IR(m[2][3]) ^= IR(m[3][2]); } //! Computes a cofactor. Used for matrix inversion. float CoFactor(udword row, udword col) const; //! Computes the determinant of the matrix. float Determinant() const; //! Inverts the matrix. Determinant must be different from zero, else matrix can't be inverted. Matrix4x4& Invert(); // Matrix& ComputeAxisMatrix(Point& axis, float angle); // Cast operators //! Casts a Matrix4x4 to a Matrix3x3. inline_ operator Matrix3x3() const { return Matrix3x3( 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]); } //! Casts a Matrix4x4 to a Quat. operator Quat() const; //! Casts a Matrix4x4 to a PR. operator PR() const; // Arithmetic operators //! Operator for Matrix4x4 Plus = Matrix4x4 + Matrix4x4; inline_ Matrix4x4 operator+(const Matrix4x4& mat) const { return Matrix4x4( m[0][0]+mat.m[0][0], m[0][1]+mat.m[0][1], m[0][2]+mat.m[0][2], m[0][3]+mat.m[0][3], m[1][0]+mat.m[1][0], m[1][1]+mat.m[1][1], m[1][2]+mat.m[1][2], m[1][3]+mat.m[1][3], m[2][0]+mat.m[2][0], m[2][1]+mat.m[2][1], m[2][2]+mat.m[2][2], m[2][3]+mat.m[2][3], m[3][0]+mat.m[3][0], m[3][1]+mat.m[3][1], m[3][2]+mat.m[3][2], m[3][3]+mat.m[3][3]); } //! Operator for Matrix4x4 Minus = Matrix4x4 - Matrix4x4; inline_ Matrix4x4 operator-(const Matrix4x4& mat) const { return Matrix4x4( m[0][0]-mat.m[0][0], m[0][1]-mat.m[0][1], m[0][2]-mat.m[0][2], m[0][3]-mat.m[0][3], m[1][0]-mat.m[1][0], m[1][1]-mat.m[1][1], m[1][2]-mat.m[1][2], m[1][3]-mat.m[1][3], m[2][0]-mat.m[2][0], m[2][1]-mat.m[2][1], m[2][2]-mat.m[2][2], m[2][3]-mat.m[2][3], m[3][0]-mat.m[3][0], m[3][1]-mat.m[3][1], m[3][2]-mat.m[3][2], m[3][3]-mat.m[3][3]); } //! Operator for Matrix4x4 Mul = Matrix4x4 * Matrix4x4; inline_ Matrix4x4 operator*(const Matrix4x4& mat) const { return Matrix4x4( m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0] + m[0][3]*mat.m[3][0], m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1] + m[0][3]*mat.m[3][1], m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2] + m[0][3]*mat.m[3][2], m[0][0]*mat.m[0][3] + m[0][1]*mat.m[1][3] + m[0][2]*mat.m[2][3] + m[0][3]*mat.m[3][3], m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0] + m[1][3]*mat.m[3][0], m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1] + m[1][3]*mat.m[3][1], m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2] + m[1][3]*mat.m[3][2], m[1][0]*mat.m[0][3] + m[1][1]*mat.m[1][3] + m[1][2]*mat.m[2][3] + m[1][3]*mat.m[3][3], m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0] + m[2][3]*mat.m[3][0], m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1] + m[2][3]*mat.m[3][1], m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2] + m[2][3]*mat.m[3][2], m[2][0]*mat.m[0][3] + m[2][1]*mat.m[1][3] + m[2][2]*mat.m[2][3] + m[2][3]*mat.m[3][3], m[3][0]*mat.m[0][0] + m[3][1]*mat.m[1][0] + m[3][2]*mat.m[2][0] + m[3][3]*mat.m[3][0], m[3][0]*mat.m[0][1] + m[3][1]*mat.m[1][1] + m[3][2]*mat.m[2][1] + m[3][3]*mat.m[3][1], m[3][0]*mat.m[0][2] + m[3][1]*mat.m[1][2] + m[3][2]*mat.m[2][2] + m[3][3]*mat.m[3][2], m[3][0]*mat.m[0][3] + m[3][1]*mat.m[1][3] + m[3][2]*mat.m[2][3] + m[3][3]*mat.m[3][3]); } //! Operator for HPoint Mul = Matrix4x4 * HPoint; inline_ HPoint operator*(const HPoint& v) const { return HPoint(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v, GetRow(3)|v); } //! Operator for Point Mul = Matrix4x4 * Point; inline_ Point operator*(const Point& v) const { return Point( m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3], m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3], m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3] ); } //! Operator for Matrix4x4 Scale = Matrix4x4 * float; inline_ Matrix4x4 operator*(float s) const { return Matrix4x4( m[0][0]*s, m[0][1]*s, m[0][2]*s, m[0][3]*s, m[1][0]*s, m[1][1]*s, m[1][2]*s, m[1][3]*s, m[2][0]*s, m[2][1]*s, m[2][2]*s, m[2][3]*s, m[3][0]*s, m[3][1]*s, m[3][2]*s, m[3][3]*s); } //! Operator for Matrix4x4 Scale = float * Matrix4x4; inline_ friend Matrix4x4 operator*(float s, const Matrix4x4& mat) { return Matrix4x4( s*mat.m[0][0], s*mat.m[0][1], s*mat.m[0][2], s*mat.m[0][3], s*mat.m[1][0], s*mat.m[1][1], s*mat.m[1][2], s*mat.m[1][3], s*mat.m[2][0], s*mat.m[2][1], s*mat.m[2][2], s*mat.m[2][3], s*mat.m[3][0], s*mat.m[3][1], s*mat.m[3][2], s*mat.m[3][3]); } //! Operator for Matrix4x4 Div = Matrix4x4 / float; inline_ Matrix4x4 operator/(float s) const { if(s) s = 1.0f / s; return Matrix4x4( m[0][0]*s, m[0][1]*s, m[0][2]*s, m[0][3]*s, m[1][0]*s, m[1][1]*s, m[1][2]*s, m[1][3]*s, m[2][0]*s, m[2][1]*s, m[2][2]*s, m[2][3]*s, m[3][0]*s, m[3][1]*s, m[3][2]*s, m[3][3]*s); } //! Operator for Matrix4x4 Div = float / Matrix4x4; inline_ friend Matrix4x4 operator/(float s, const Matrix4x4& mat) { return Matrix4x4( s/mat.m[0][0], s/mat.m[0][1], s/mat.m[0][2], s/mat.m[0][3], s/mat.m[1][0], s/mat.m[1][1], s/mat.m[1][2], s/mat.m[1][3], s/mat.m[2][0], s/mat.m[2][1], s/mat.m[2][2], s/mat.m[2][3], s/mat.m[3][0], s/mat.m[3][1], s/mat.m[3][2], s/mat.m[3][3]); } //! Operator for Matrix4x4 += Matrix4x4; inline_ Matrix4x4& operator+=(const Matrix4x4& mat) { m[0][0]+=mat.m[0][0]; m[0][1]+=mat.m[0][1]; m[0][2]+=mat.m[0][2]; m[0][3]+=mat.m[0][3]; m[1][0]+=mat.m[1][0]; m[1][1]+=mat.m[1][1]; m[1][2]+=mat.m[1][2]; m[1][3]+=mat.m[1][3]; m[2][0]+=mat.m[2][0]; m[2][1]+=mat.m[2][1]; m[2][2]+=mat.m[2][2]; m[2][3]+=mat.m[2][3]; m[3][0]+=mat.m[3][0]; m[3][1]+=mat.m[3][1]; m[3][2]+=mat.m[3][2]; m[3][3]+=mat.m[3][3]; return *this; } //! Operator for Matrix4x4 -= Matrix4x4; inline_ Matrix4x4& operator-=(const Matrix4x4& mat) { m[0][0]-=mat.m[0][0]; m[0][1]-=mat.m[0][1]; m[0][2]-=mat.m[0][2]; m[0][3]-=mat.m[0][3]; m[1][0]-=mat.m[1][0]; m[1][1]-=mat.m[1][1]; m[1][2]-=mat.m[1][2]; m[1][3]-=mat.m[1][3]; m[2][0]-=mat.m[2][0]; m[2][1]-=mat.m[2][1]; m[2][2]-=mat.m[2][2]; m[2][3]-=mat.m[2][3]; m[3][0]-=mat.m[3][0]; m[3][1]-=mat.m[3][1]; m[3][2]-=mat.m[3][2]; m[3][3]-=mat.m[3][3]; return *this; } //! Operator for Matrix4x4 *= Matrix4x4; Matrix4x4& operator*=(const Matrix4x4& mat) { HPoint TempRow; GetRow(0, TempRow); m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0]; m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1]; m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2]; m[0][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3]; GetRow(1, TempRow); m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0]; m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1]; m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2]; m[1][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3]; GetRow(2, TempRow); m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0]; m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1]; m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2]; m[2][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3]; GetRow(3, TempRow); m[3][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0]; m[3][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1]; m[3][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2]; m[3][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3]; return *this; } //! Operator for Matrix4x4 *= float; inline_ Matrix4x4& operator*=(float s) { m[0][0]*=s; m[0][1]*=s; m[0][2]*=s; m[0][3]*=s; m[1][0]*=s; m[1][1]*=s; m[1][2]*=s; m[1][3]*=s; m[2][0]*=s; m[2][1]*=s; m[2][2]*=s; m[2][3]*=s; m[3][0]*=s; m[3][1]*=s; m[3][2]*=s; m[3][3]*=s; return *this; } //! Operator for Matrix4x4 /= float; inline_ Matrix4x4& operator/=(float s) { if(s) s = 1.0f / s; m[0][0]*=s; m[0][1]*=s; m[0][2]*=s; m[0][3]*=s; m[1][0]*=s; m[1][1]*=s; m[1][2]*=s; m[1][3]*=s; m[2][0]*=s; m[2][1]*=s; m[2][2]*=s; m[2][3]*=s; m[3][0]*=s; m[3][1]*=s; m[3][2]*=s; m[3][3]*=s; return *this; } inline_ const HPoint& operator[](int row) const { return *(const HPoint*)&m[row][0]; } inline_ HPoint& operator[](int row) { return *(HPoint*)&m[row][0]; } public: float m[4][4]; }; //! Quickly rotates & translates a vector, using the 4x3 part of a 4x4 matrix inline_ void TransformPoint4x3(Point& dest, const Point& source, const Matrix4x4& rot) { dest.x = rot.m[3][0] + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0]; dest.y = rot.m[3][1] + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1]; dest.z = rot.m[3][2] + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2]; } //! Quickly rotates a vector, using the 3x3 part of a 4x4 matrix inline_ void TransformPoint3x3(Point& dest, const Point& source, const Matrix4x4& rot) { dest.x = source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0]; dest.y = source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1]; dest.z = source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2]; } ICEMATHS_API void InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src); #endif // __ICEMATRIX4X4_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceMemoryMacros.h000066400000000000000000000076761207742442300244500ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains all memory macros. * \file IceMemoryMacros.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEMEMORYMACROS_H__ #define __ICEMEMORYMACROS_H__ #undef ZeroMemory #undef CopyMemory #undef MoveMemory #undef FillMemory //! Clears a buffer. //! \param addr [in] buffer address //! \param size [in] buffer length //! \see FillMemory //! \see StoreDwords //! \see CopyMemory //! \see MoveMemory inline_ void ZeroMemory(void* addr, udword size) { memset(addr, 0, size); } //! Fills a buffer with a given byte. //! \param addr [in] buffer address //! \param size [in] buffer length //! \param val [in] the byte value //! \see StoreDwords //! \see ZeroMemory //! \see CopyMemory //! \see MoveMemory inline_ void FillMemory(void* dest, udword size, ubyte val) { memset(dest, val, size); } //! Fills a buffer with a given dword. //! \param addr [in] buffer address //! \param nb [in] number of dwords to write //! \param value [in] the dword value //! \see FillMemory //! \see ZeroMemory //! \see CopyMemory //! \see MoveMemory //! \warning writes nb*4 bytes ! inline_ void StoreDwords(udword* dest, udword nb, udword value) { // The asm code below **SHOULD** be equivalent to one of those C versions // or the other if your compiled is good: (checked on VC++ 6.0) // // 1) while(nb--) *dest++ = value; // // 2) for(udword i=0;iRelease(); (x) = null; } //!< Safe D3D-style release #define SAFE_DESTRUCT(x) if (x) { (x)->SelfDestruct(); (x) = null; } //!< Safe ICE-style release #ifdef __ICEERROR_H__ #define CHECKALLOC(x) if(!x) return SetIceError("Out of memory.", EC_OUT_OF_MEMORY); //!< Standard alloc checking. HANDLE WITH CARE. #else #define CHECKALLOC(x) if(!x) return false; #endif //! Standard allocation cycle #define SAFE_ALLOC(ptr, type, count) DELETEARRAY(ptr); ptr = new type[count]; CHECKALLOC(ptr); #endif // __ICEMEMORYMACROS_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceOBB.cpp000066400000000000000000000303171207742442300227540ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains OBB-related code. * \file IceOBB.cpp * \author Pierre Terdiman * \date January, 29, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * An Oriented Bounding Box (OBB). * \class OBB * \author Pierre Terdiman * \version 1.0 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if a point is contained within the OBB. * \param p [in] the world point to test * \return true if inside the OBB */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool OBB::ContainsPoint(const Point& p) const { // Point in OBB test using lazy evaluation and early exits // Translate to box space Point RelPoint = p - mCenter; // Point * mRot maps from box space to world space // mRot * Point maps from world space to box space (what we need here) float f = mRot.m[0][0] * RelPoint.x + mRot.m[0][1] * RelPoint.y + mRot.m[0][2] * RelPoint.z; if(f >= mExtents.x || f <= -mExtents.x) return false; f = mRot.m[1][0] * RelPoint.x + mRot.m[1][1] * RelPoint.y + mRot.m[1][2] * RelPoint.z; if(f >= mExtents.y || f <= -mExtents.y) return false; f = mRot.m[2][0] * RelPoint.x + mRot.m[2][1] * RelPoint.y + mRot.m[2][2] * RelPoint.z; if(f >= mExtents.z || f <= -mExtents.z) return false; return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds an OBB from an AABB and a world transform. * \param aabb [in] the aabb * \param mat [in] the world transform */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void OBB::Create(const AABB& aabb, const Matrix4x4& mat) { // Note: must be coherent with Rotate() aabb.GetCenter(mCenter); aabb.GetExtents(mExtents); // Here we have the same as OBB::Rotate(mat) where the obb is (mCenter, mExtents, Identity). // So following what's done in Rotate: // - x-form the center mCenter *= mat; // - combine rotation with identity, i.e. just use given matrix mRot = mat; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the obb planes. * \param planes [out] 6 box planes * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool OBB::ComputePlanes(Plane* planes) const { // Checkings if(!planes) return false; Point Axis0 = mRot[0]; Point Axis1 = mRot[1]; Point Axis2 = mRot[2]; // Writes normals planes[0].n = Axis0; planes[1].n = -Axis0; planes[2].n = Axis1; planes[3].n = -Axis1; planes[4].n = Axis2; planes[5].n = -Axis2; // Compute a point on each plane Point p0 = mCenter + Axis0 * mExtents.x; Point p1 = mCenter - Axis0 * mExtents.x; Point p2 = mCenter + Axis1 * mExtents.y; Point p3 = mCenter - Axis1 * mExtents.y; Point p4 = mCenter + Axis2 * mExtents.z; Point p5 = mCenter - Axis2 * mExtents.z; // Compute d planes[0].d = -(planes[0].n|p0); planes[1].d = -(planes[1].n|p1); planes[2].d = -(planes[2].n|p2); planes[3].d = -(planes[3].n|p3); planes[4].d = -(planes[4].n|p4); planes[5].d = -(planes[5].n|p5); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the obb points. * \param pts [out] 8 box points * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool OBB::ComputePoints(Point* pts) const { // Checkings if(!pts) return false; Point Axis0 = mRot[0]; Point Axis1 = mRot[1]; Point Axis2 = mRot[2]; Axis0 *= mExtents.x; Axis1 *= mExtents.y; Axis2 *= mExtents.z; // 7+------+6 0 = --- // /| /| 1 = +-- // / | / | 2 = ++- // / 4+---/--+5 3 = -+- // 3+------+2 / y z 4 = --+ // | / | / | / 5 = +-+ // |/ |/ |/ 6 = +++ // 0+------+1 *---x 7 = -++ pts[0] = mCenter - Axis0 - Axis1 - Axis2; pts[1] = mCenter + Axis0 - Axis1 - Axis2; pts[2] = mCenter + Axis0 + Axis1 - Axis2; pts[3] = mCenter - Axis0 + Axis1 - Axis2; pts[4] = mCenter - Axis0 - Axis1 + Axis2; pts[5] = mCenter + Axis0 - Axis1 + Axis2; pts[6] = mCenter + Axis0 + Axis1 + Axis2; pts[7] = mCenter - Axis0 + Axis1 + Axis2; return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes vertex normals. * \param pts [out] 8 box points * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool OBB::ComputeVertexNormals(Point* pts) const { static float VertexNormals[] = { -INVSQRT3, -INVSQRT3, -INVSQRT3, INVSQRT3, -INVSQRT3, -INVSQRT3, INVSQRT3, INVSQRT3, -INVSQRT3, -INVSQRT3, INVSQRT3, -INVSQRT3, -INVSQRT3, -INVSQRT3, INVSQRT3, INVSQRT3, -INVSQRT3, INVSQRT3, INVSQRT3, INVSQRT3, INVSQRT3, -INVSQRT3, INVSQRT3, INVSQRT3 }; if(!pts) return false; const Point* VN = (const Point*)VertexNormals; for(udword i=0;i<8;i++) { pts[i] = VN[i] * mRot; } return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Returns edges. * \return 24 indices (12 edges) indexing the list returned by ComputePoints() */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// const udword* OBB::GetEdges() const { static udword Indices[] = { 0, 1, 1, 2, 2, 3, 3, 0, 7, 6, 6, 5, 5, 4, 4, 7, 1, 5, 6, 2, 3, 7, 4, 0 }; return Indices; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Returns local edge normals. * \return edge normals in local space */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// const Point* OBB::GetLocalEdgeNormals() const { static float EdgeNormals[] = { 0, -INVSQRT2, -INVSQRT2, // 0-1 INVSQRT2, 0, -INVSQRT2, // 1-2 0, INVSQRT2, -INVSQRT2, // 2-3 -INVSQRT2, 0, -INVSQRT2, // 3-0 0, INVSQRT2, INVSQRT2, // 7-6 INVSQRT2, 0, INVSQRT2, // 6-5 0, -INVSQRT2, INVSQRT2, // 5-4 -INVSQRT2, 0, INVSQRT2, // 4-7 INVSQRT2, -INVSQRT2, 0, // 1-5 INVSQRT2, INVSQRT2, 0, // 6-2 -INVSQRT2, INVSQRT2, 0, // 3-7 -INVSQRT2, -INVSQRT2, 0 // 4-0 }; return (const Point*)EdgeNormals; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Returns world edge normal * \param edge_index [in] 0 <= edge index < 12 * \param world_normal [out] edge normal in world space */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void OBB::ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const { ASSERT(edge_index<12); world_normal = GetLocalEdgeNormals()[edge_index] * mRot; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes an LSS surrounding the OBB. * \param lss [out] the LSS */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void OBB::ComputeLSS(LSS& lss) const { Point Axis0 = mRot[0]; Point Axis1 = mRot[1]; Point Axis2 = mRot[2]; switch(mExtents.LargestAxis()) { case 0: lss.mRadius = (mExtents.y + mExtents.z)*0.5f; lss.mP0 = mCenter + Axis0 * (mExtents.x - lss.mRadius); lss.mP1 = mCenter - Axis0 * (mExtents.x - lss.mRadius); break; case 1: lss.mRadius = (mExtents.x + mExtents.z)*0.5f; lss.mP0 = mCenter + Axis1 * (mExtents.y - lss.mRadius); lss.mP1 = mCenter - Axis1 * (mExtents.y - lss.mRadius); break; case 2: lss.mRadius = (mExtents.x + mExtents.y)*0.5f; lss.mP0 = mCenter + Axis2 * (mExtents.z - lss.mRadius); lss.mP1 = mCenter - Axis2 * (mExtents.z - lss.mRadius); break; } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the OBB is inside another OBB. * \param box [in] the other OBB * \return TRUE if we're inside the other box */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// BOOL OBB::IsInside(const OBB& box) const { // Make a 4x4 from the box & inverse it Matrix4x4 M0Inv; { Matrix4x4 M0 = box.mRot; M0.SetTrans(box.mCenter); InvertPRMatrix(M0Inv, M0); } // With our inversed 4x4, create box1 in space of box0 OBB _1in0; Rotate(M0Inv, _1in0); // This should cancel out box0's rotation, i.e. it's now an AABB. // => Center(0,0,0), Rot(identity) // The two boxes are in the same space so now we can compare them. // Create the AABB of (box1 in space of box0) const Matrix3x3& mtx = _1in0.mRot; float f = fabsf(mtx.m[0][0] * mExtents.x) + fabsf(mtx.m[1][0] * mExtents.y) + fabsf(mtx.m[2][0] * mExtents.z) - box.mExtents.x; if(f > _1in0.mCenter.x) return FALSE; if(-f < _1in0.mCenter.x) return FALSE; f = fabsf(mtx.m[0][1] * mExtents.x) + fabsf(mtx.m[1][1] * mExtents.y) + fabsf(mtx.m[2][1] * mExtents.z) - box.mExtents.y; if(f > _1in0.mCenter.y) return FALSE; if(-f < _1in0.mCenter.y) return FALSE; f = fabsf(mtx.m[0][2] * mExtents.x) + fabsf(mtx.m[1][2] * mExtents.y) + fabsf(mtx.m[2][2] * mExtents.z) - box.mExtents.z; if(f > _1in0.mCenter.z) return FALSE; if(-f < _1in0.mCenter.z) return FALSE; return TRUE; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceOBB.h000066400000000000000000000230001207742442300224100ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains OBB-related code. (oriented bounding box) * \file IceOBB.h * \author Pierre Terdiman * \date January, 13, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEOBB_H__ #define __ICEOBB_H__ // Forward declarations class LSS; class ICEMATHS_API OBB { public: //! Constructor inline_ OBB() {} //! Constructor inline_ OBB(const Point& center, const Point& extents, const Matrix3x3& rot) : mCenter(center), mExtents(extents), mRot(rot) {} //! Destructor inline_ ~OBB() {} /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups an empty OBB. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SetEmpty() { mCenter.Zero(); mExtents.Set(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); mRot.Identity(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Tests if a point is contained within the OBB. * \param p [in] the world point to test * \return true if inside the OBB */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool ContainsPoint(const Point& p) const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds an OBB from an AABB and a world transform. * \param aabb [in] the aabb * \param mat [in] the world transform */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Create(const AABB& aabb, const Matrix4x4& mat); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recomputes the OBB after an arbitrary transform by a 4x4 matrix. * \param mtx [in] the transform matrix * \param obb [out] the transformed OBB */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void Rotate(const Matrix4x4& mtx, OBB& obb) const { // The extents remain constant obb.mExtents = mExtents; // The center gets x-formed obb.mCenter = mCenter * mtx; // Combine rotations obb.mRot = mRot * Matrix3x3(mtx); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the OBB is valid. * \return true if the box is valid */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL IsValid() const { // Consistency condition for (Center, Extents) boxes: Extents >= 0.0f if(mExtents.x < 0.0f) return FALSE; if(mExtents.y < 0.0f) return FALSE; if(mExtents.z < 0.0f) return FALSE; return TRUE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the obb planes. * \param planes [out] 6 box planes * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool ComputePlanes(Plane* planes) const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the obb points. * \param pts [out] 8 box points * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool ComputePoints(Point* pts) const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes vertex normals. * \param pts [out] 8 box points * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool ComputeVertexNormals(Point* pts) const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Returns edges. * \return 24 indices (12 edges) indexing the list returned by ComputePoints() */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// const udword* GetEdges() const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Returns local edge normals. * \return edge normals in local space */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// const Point* GetLocalEdgeNormals() const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Returns world edge normal * \param edge_index [in] 0 <= edge index < 12 * \param world_normal [out] edge normal in world space */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes an LSS surrounding the OBB. * \param lss [out] the LSS */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void ComputeLSS(LSS& lss) const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the OBB is inside another OBB. * \param box [in] the other OBB * \return TRUE if we're inside the other box */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// BOOL IsInside(const OBB& box) const; inline_ const Point& GetCenter() const { return mCenter; } inline_ const Point& GetExtents() const { return mExtents; } inline_ const Matrix3x3& GetRot() const { return mRot; } inline_ void GetRotatedExtents(Matrix3x3& extents) const { extents = mRot; extents.Scale(mExtents); } Point mCenter; //!< B for Box Point mExtents; //!< B for Bounding Matrix3x3 mRot; //!< O for Oriented // Orientation is stored in row-major format, // i.e. rows = eigen vectors of the covariance matrix }; #endif // __ICEOBB_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IcePairs.h000066400000000000000000000033731207742442300230770ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a simple pair class. * \file IcePairs.h * \author Pierre Terdiman * \date January, 13, 2003 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEPAIRS_H__ #define __ICEPAIRS_H__ //! A generic couple structure struct ICECORE_API Pair { inline_ Pair() {} inline_ Pair(udword i0, udword i1) : id0(i0), id1(i1) {} udword id0; //!< First index of the pair udword id1; //!< Second index of the pair }; class ICECORE_API Pairs : private Container { public: // Constructor / Destructor Pairs() {} ~Pairs() {} inline_ udword GetNbPairs() const { return GetNbEntries()>>1; } inline_ const Pair* GetPairs() const { return (const Pair*)GetEntries(); } inline_ const Pair* GetPair(udword i) const { return (const Pair*)&GetEntries()[i+i]; } inline_ BOOL HasPairs() const { return IsNotEmpty(); } inline_ void ResetPairs() { Reset(); } inline_ void DeleteLastPair() { DeleteLastEntry(); DeleteLastEntry(); } inline_ void AddPair(const Pair& p) { Add(p.id0).Add(p.id1); } inline_ void AddPair(udword id0, udword id1) { Add(id0).Add(id1); } }; #endif // __ICEPAIRS_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IcePlane.cpp000066400000000000000000000037641207742442300234170ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for planes. * \file IcePlane.cpp * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Plane class. * \class Plane * \author Pierre Terdiman * \version 1.0 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the plane equation from 3 points. * \param p0 [in] first point * \param p1 [in] second point * \param p2 [in] third point * \return Self-reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Plane& Plane::Set(const Point& p0, const Point& p1, const Point& p2) { Point Edge0 = p1 - p0; Point Edge1 = p2 - p0; n = Edge0 ^ Edge1; n.Normalize(); d = -(p0 | n); return *this; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IcePlane.h000066400000000000000000000115661207742442300230630ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for planes. * \file IcePlane.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEPLANE_H__ #define __ICEPLANE_H__ #define PLANE_EPSILON (1.0e-7f) class ICEMATHS_API Plane { public: //! Constructor inline_ Plane() { } //! Constructor from a normal and a distance inline_ Plane(float nx, float ny, float nz, float d) { Set(nx, ny, nz, d); } //! Constructor from a point on the plane and a normal inline_ Plane(const Point& p, const Point& n) { Set(p, n); } //! Constructor from three points inline_ Plane(const Point& p0, const Point& p1, const Point& p2) { Set(p0, p1, p2); } //! Constructor from a normal and a distance inline_ Plane(const Point& _n, float _d) { n = _n; d = _d; } //! Copy constructor inline_ Plane(const Plane& plane) : n(plane.n), d(plane.d) { } //! Destructor inline_ ~Plane() { } inline_ Plane& Zero() { n.Zero(); d = 0.0f; return *this; } inline_ Plane& Set(float nx, float ny, float nz, float _d) { n.Set(nx, ny, nz); d = _d; return *this; } inline_ Plane& Set(const Point& p, const Point& _n) { n = _n; d = - p | _n; return *this; } Plane& Set(const Point& p0, const Point& p1, const Point& p2); inline_ float Distance(const Point& p) const { return (p | n) + d; } inline_ bool Belongs(const Point& p) const { return fabsf(Distance(p)) < PLANE_EPSILON; } inline_ void Normalize() { float Denom = 1.0f / n.Magnitude(); n.x *= Denom; n.y *= Denom; n.z *= Denom; d *= Denom; } public: // Members Point n; //!< The normal to the plane float d; //!< The distance from the origin // Cast operators inline_ operator Point() const { return n; } inline_ operator HPoint() const { return HPoint(n, d); } // Arithmetic operators inline_ Plane operator*(const Matrix4x4& m) const { // Old code from Irion. Kept for reference. Plane Ret(*this); return Ret *= m; } inline_ Plane& operator*=(const Matrix4x4& m) { // Old code from Irion. Kept for reference. Point n2 = HPoint(n, 0.0f) * m; d = -((Point) (HPoint( -d*n, 1.0f ) * m) | n2); n = n2; return *this; } }; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Transforms a plane by a 4x4 matrix. Same as Plane * Matrix4x4 operator, but faster. * \param transformed [out] transformed plane * \param plane [in] source plane * \param transform [in] transform matrix * \warning the plane normal must be unit-length */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void TransformPlane(Plane& transformed, const Plane& plane, const Matrix4x4& transform) { // Rotate the normal using the rotation part of the 4x4 matrix transformed.n = plane.n * Matrix3x3(transform); // Compute new d transformed.d = plane.d - (Point(transform.GetTrans())|transformed.n); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Transforms a plane by a 4x4 matrix. Same as Plane * Matrix4x4 operator, but faster. * \param plane [in/out] source plane (transformed on return) * \param transform [in] transform matrix * \warning the plane normal must be unit-length */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void TransformPlane(Plane& plane, const Matrix4x4& transform) { // Rotate the normal using the rotation part of the 4x4 matrix plane.n *= Matrix3x3(transform); // Compute new d plane.d -= Point(transform.GetTrans())|plane.n; } #endif // __ICEPLANE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IcePoint.cpp000066400000000000000000000164041207742442300234440ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for 3D vectors. * \file IcePoint.cpp * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * 3D point. * * The name is "Point" instead of "Vector" since a vector is N-dimensional, whereas a point is an implicit "vector of dimension 3". * So the choice was between "Point" and "Vector3", the first one looked better (IMHO). * * Some people, then, use a typedef to handle both points & vectors using the same class: typedef Point Vector3; * This is bad since it opens the door to a lot of confusion while reading the code. I know it may sounds weird but check this out: * * \code * Point P0,P1 = some 3D points; * Point Delta = P1 - P0; * \endcode * * This compiles fine, although you should have written: * * \code * Point P0,P1 = some 3D points; * Vector3 Delta = P1 - P0; * \endcode * * Subtle things like this are not caught at compile-time, and when you find one in the code, you never know whether it's a mistake * from the author or something you don't get. * * One way to handle it at compile-time would be to use different classes for Point & Vector3, only overloading operator "-" for vectors. * But then, you get a lot of redundant code in thoses classes, and basically it's really a lot of useless work. * * Another way would be to use homogeneous points: w=1 for points, w=0 for vectors. That's why the HPoint class exists. Now, to store * your model's vertices and in most cases, you really want to use Points to save ram. * * \class Point * \author Pierre Terdiman * \version 1.0 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Creates a positive unit random vector. * \return Self-reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Point& Point::PositiveUnitRandomVector() { x = UnitRandomFloat(); y = UnitRandomFloat(); z = UnitRandomFloat(); Normalize(); return *this; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Creates a unit random vector. * \return Self-reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Point& Point::UnitRandomVector() { x = UnitRandomFloat() - 0.5f; y = UnitRandomFloat() - 0.5f; z = UnitRandomFloat() - 0.5f; Normalize(); return *this; } // Cast operator // WARNING: not inlined Point::operator HPoint() const { return HPoint(x, y, z, 0.0f); } Point& Point::Refract(const Point& eye, const Point& n, float refractindex, Point& refracted) { // Point EyePt = eye position // Point p = current vertex // Point n = vertex normal // Point rv = refracted vector // Eye vector - doesn't need to be normalized Point Env; Env.x = eye.x - x; Env.y = eye.y - y; Env.z = eye.z - z; float NDotE = n|Env; float NDotN = n|n; NDotE /= refractindex; // Refracted vector refracted = n*NDotE - Env*NDotN; return *this; } Point& Point::ProjectToPlane(const Plane& p) { *this-= (p.d + (*this|p.n))*p.n; return *this; } void Point::ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const { projected = HPoint(x, y, z, 1.0f) * mat; projected.w = 1.0f / projected.w; projected.x*=projected.w; projected.y*=projected.w; projected.z*=projected.w; projected.x *= halfrenderwidth; projected.x += halfrenderwidth; projected.y *= -halfrenderheight; projected.y += halfrenderheight; } void Point::SetNotUsed() { // We use a particular integer pattern : 0xffffffff everywhere. This is a NAN. IR(x) = 0xffffffff; IR(y) = 0xffffffff; IR(z) = 0xffffffff; } BOOL Point::IsNotUsed() const { if(IR(x)!=0xffffffff) return FALSE; if(IR(y)!=0xffffffff) return FALSE; if(IR(z)!=0xffffffff) return FALSE; return TRUE; } Point& Point::Mult(const Matrix3x3& mat, const Point& a) { x = a.x * mat.m[0][0] + a.y * mat.m[0][1] + a.z * mat.m[0][2]; y = a.x * mat.m[1][0] + a.y * mat.m[1][1] + a.z * mat.m[1][2]; z = a.x * mat.m[2][0] + a.y * mat.m[2][1] + a.z * mat.m[2][2]; return *this; } Point& Point::Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2) { x = a1.x * mat1.m[0][0] + a1.y * mat1.m[0][1] + a1.z * mat1.m[0][2] + a2.x * mat2.m[0][0] + a2.y * mat2.m[0][1] + a2.z * mat2.m[0][2]; y = a1.x * mat1.m[1][0] + a1.y * mat1.m[1][1] + a1.z * mat1.m[1][2] + a2.x * mat2.m[1][0] + a2.y * mat2.m[1][1] + a2.z * mat2.m[1][2]; z = a1.x * mat1.m[2][0] + a1.y * mat1.m[2][1] + a1.z * mat1.m[2][2] + a2.x * mat2.m[2][0] + a2.y * mat2.m[2][1] + a2.z * mat2.m[2][2]; return *this; } Point& Point::Mac(const Matrix3x3& mat, const Point& a) { x += a.x * mat.m[0][0] + a.y * mat.m[0][1] + a.z * mat.m[0][2]; y += a.x * mat.m[1][0] + a.y * mat.m[1][1] + a.z * mat.m[1][2]; z += a.x * mat.m[2][0] + a.y * mat.m[2][1] + a.z * mat.m[2][2]; return *this; } Point& Point::TransMult(const Matrix3x3& mat, const Point& a) { x = a.x * mat.m[0][0] + a.y * mat.m[1][0] + a.z * mat.m[2][0]; y = a.x * mat.m[0][1] + a.y * mat.m[1][1] + a.z * mat.m[2][1]; z = a.x * mat.m[0][2] + a.y * mat.m[1][2] + a.z * mat.m[2][2]; return *this; } Point& Point::Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos) { x = r.x * rotpos.m[0][0] + r.y * rotpos.m[0][1] + r.z * rotpos.m[0][2] + linpos.x; y = r.x * rotpos.m[1][0] + r.y * rotpos.m[1][1] + r.z * rotpos.m[1][2] + linpos.y; z = r.x * rotpos.m[2][0] + r.y * rotpos.m[2][1] + r.z * rotpos.m[2][2] + linpos.z; return *this; } Point& Point::InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos) { float sx = r.x - linpos.x; float sy = r.y - linpos.y; float sz = r.z - linpos.z; x = sx * rotpos.m[0][0] + sy * rotpos.m[1][0] + sz * rotpos.m[2][0]; y = sx * rotpos.m[0][1] + sy * rotpos.m[1][1] + sz * rotpos.m[2][1]; z = sx * rotpos.m[0][2] + sy * rotpos.m[1][2] + sz * rotpos.m[2][2]; return *this; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IcePoint.h000066400000000000000000000462611207742442300231150ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for 3D vectors. * \file IcePoint.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEPOINT_H__ #define __ICEPOINT_H__ // Forward declarations class HPoint; class Plane; class Matrix3x3; class Matrix4x4; #define CROSS2D(a, b) (a.x*b.y - b.x*a.y) const float EPSILON2 = 1.0e-20f; class ICEMATHS_API Point { public: //! Empty constructor inline_ Point() {} //! Constructor from a single float // inline_ Point(float val) : x(val), y(val), z(val) {} // Removed since it introduced the nasty "Point T = *Matrix4x4.GetTrans();" bug....... //! Constructor from floats inline_ Point(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {} //! Constructor from array inline_ Point(const float f[3]) : x(f[_X]), y(f[_Y]), z(f[_Z]) {} //! Copy constructor inline_ Point(const Point& p) : x(p.x), y(p.y), z(p.z) {} //! Destructor inline_ ~Point() {} //! Clears the vector inline_ Point& Zero() { x = y = z = 0.0f; return *this; } //! + infinity inline_ Point& SetPlusInfinity() { x = y = z = MAX_FLOAT; return *this; } //! - infinity inline_ Point& SetMinusInfinity() { x = y = z = MIN_FLOAT; return *this; } //! Sets positive unit random vector Point& PositiveUnitRandomVector(); //! Sets unit random vector Point& UnitRandomVector(); //! Assignment from values inline_ Point& Set(float _x, float _y, float _z) { x = _x; y = _y; z = _z; return *this; } //! Assignment from array inline_ Point& Set(const float f[3]) { x = f[_X]; y = f[_Y]; z = f[_Z]; return *this; } //! Assignment from another point inline_ Point& Set(const Point& src) { x = src.x; y = src.y; z = src.z; return *this; } //! Adds a vector inline_ Point& Add(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; } //! Adds a vector inline_ Point& Add(float _x, float _y, float _z) { x += _x; y += _y; z += _z; return *this; } //! Adds a vector inline_ Point& Add(const float f[3]) { x += f[_X]; y += f[_Y]; z += f[_Z]; return *this; } //! Adds vectors inline_ Point& Add(const Point& p, const Point& q) { x = p.x+q.x; y = p.y+q.y; z = p.z+q.z; return *this; } //! Subtracts a vector inline_ Point& Sub(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; } //! Subtracts a vector inline_ Point& Sub(float _x, float _y, float _z) { x -= _x; y -= _y; z -= _z; return *this; } //! Subtracts a vector inline_ Point& Sub(const float f[3]) { x -= f[_X]; y -= f[_Y]; z -= f[_Z]; return *this; } //! Subtracts vectors inline_ Point& Sub(const Point& p, const Point& q) { x = p.x-q.x; y = p.y-q.y; z = p.z-q.z; return *this; } //! this = -this inline_ Point& Neg() { x = -x; y = -y; z = -z; return *this; } //! this = -a inline_ Point& Neg(const Point& a) { x = -a.x; y = -a.y; z = -a.z; return *this; } //! Multiplies by a scalar inline_ Point& Mult(float s) { x *= s; y *= s; z *= s; return *this; } //! this = a * scalar inline_ Point& Mult(const Point& a, float scalar) { x = a.x * scalar; y = a.y * scalar; z = a.z * scalar; return *this; } //! this = a + b * scalar inline_ Point& Mac(const Point& a, const Point& b, float scalar) { x = a.x + b.x * scalar; y = a.y + b.y * scalar; z = a.z + b.z * scalar; return *this; } //! this = this + a * scalar inline_ Point& Mac(const Point& a, float scalar) { x += a.x * scalar; y += a.y * scalar; z += a.z * scalar; return *this; } //! this = a - b * scalar inline_ Point& Msc(const Point& a, const Point& b, float scalar) { x = a.x - b.x * scalar; y = a.y - b.y * scalar; z = a.z - b.z * scalar; return *this; } //! this = this - a * scalar inline_ Point& Msc(const Point& a, float scalar) { x -= a.x * scalar; y -= a.y * scalar; z -= a.z * scalar; return *this; } //! this = a + b * scalarb + c * scalarc inline_ Point& Mac2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc) { x = a.x + b.x * scalarb + c.x * scalarc; y = a.y + b.y * scalarb + c.y * scalarc; z = a.z + b.z * scalarb + c.z * scalarc; return *this; } //! this = a - b * scalarb - c * scalarc inline_ Point& Msc2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc) { x = a.x - b.x * scalarb - c.x * scalarc; y = a.y - b.y * scalarb - c.y * scalarc; z = a.z - b.z * scalarb - c.z * scalarc; return *this; } //! this = mat * a inline_ Point& Mult(const Matrix3x3& mat, const Point& a); //! this = mat1 * a1 + mat2 * a2 inline_ Point& Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2); //! this = this + mat * a inline_ Point& Mac(const Matrix3x3& mat, const Point& a); //! this = transpose(mat) * a inline_ Point& TransMult(const Matrix3x3& mat, const Point& a); //! Linear interpolate between two vectors: this = a + t * (b - a) inline_ Point& Lerp(const Point& a, const Point& b, float t) { x = a.x + t * (b.x - a.x); y = a.y + t * (b.y - a.y); z = a.z + t * (b.z - a.z); return *this; } //! Hermite interpolate between p1 and p2. p0 and p3 are used for finding gradient at p1 and p2. //! this = p0 * (2t^2 - t^3 - t)/2 //! + p1 * (3t^3 - 5t^2 + 2)/2 //! + p2 * (4t^2 - 3t^3 + t)/2 //! + p3 * (t^3 - t^2)/2 inline_ Point& Herp(const Point& p0, const Point& p1, const Point& p2, const Point& p3, float t) { float t2 = t * t; float t3 = t2 * t; float kp0 = (2.0f * t2 - t3 - t) * 0.5f; float kp1 = (3.0f * t3 - 5.0f * t2 + 2.0f) * 0.5f; float kp2 = (4.0f * t2 - 3.0f * t3 + t) * 0.5f; float kp3 = (t3 - t2) * 0.5f; x = p0.x * kp0 + p1.x * kp1 + p2.x * kp2 + p3.x * kp3; y = p0.y * kp0 + p1.y * kp1 + p2.y * kp2 + p3.y * kp3; z = p0.z * kp0 + p1.z * kp1 + p2.z * kp2 + p3.z * kp3; return *this; } //! this = rotpos * r + linpos inline_ Point& Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos); //! this = trans(rotpos) * (r - linpos) inline_ Point& InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos); //! Returns MIN(x, y, z); inline_ float Min() const { return MIN(x, MIN(y, z)); } //! Returns MAX(x, y, z); inline_ float Max() const { return MAX(x, MAX(y, z)); } //! Sets each element to be componentwise minimum inline_ Point& Min(const Point& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); return *this; } //! Sets each element to be componentwise maximum inline_ Point& Max(const Point& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); return *this; } //! Clamps each element inline_ Point& Clamp(float min, float max) { if(xmax) x=max; if(ymax) y=max; if(zmax) z=max; return *this; } //! Computes square magnitude inline_ float SquareMagnitude() const { return x*x + y*y + z*z; } //! Computes magnitude inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z); } //! Computes volume inline_ float Volume() const { return x * y * z; } //! Checks the point is near zero inline_ bool ApproxZero() const { return SquareMagnitude() < EPSILON2; } //! Tests for exact zero vector inline_ BOOL IsZero() const { if(IR(x) || IR(y) || IR(z)) return FALSE; return TRUE; } //! Checks point validity inline_ BOOL IsValid() const { if(!IsValidFloat(x)) return FALSE; if(!IsValidFloat(y)) return FALSE; if(!IsValidFloat(z)) return FALSE; return TRUE; } //! Slighty moves the point void Tweak(udword coord_mask, udword tweak_mask) { if(coord_mask&1) { udword Dummy = IR(x); Dummy^=tweak_mask; x = FR(Dummy); } if(coord_mask&2) { udword Dummy = IR(y); Dummy^=tweak_mask; y = FR(Dummy); } if(coord_mask&4) { udword Dummy = IR(z); Dummy^=tweak_mask; z = FR(Dummy); } } #define TWEAKMASK 0x3fffff #define TWEAKNOTMASK ~TWEAKMASK //! Slighty moves the point out inline_ void TweakBigger() { udword Dummy = (IR(x)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy); Dummy = (IR(y)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy); Dummy = (IR(z)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy); } //! Slighty moves the point in inline_ void TweakSmaller() { udword Dummy = (IR(x)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy); Dummy = (IR(y)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy); Dummy = (IR(z)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy); } //! Normalizes the vector inline_ Point& Normalize() { float M = x*x + y*y + z*z; if(M) { M = 1.0f / sqrtf(M); x *= M; y *= M; z *= M; } return *this; } //! Sets vector length inline_ Point& SetLength(float length) { float NewLength = length / Magnitude(); x *= NewLength; y *= NewLength; z *= NewLength; return *this; } //! Clamps vector length inline_ Point& ClampLength(float limit_length) { if(limit_length>=0.0f) // Magnitude must be positive { float CurrentSquareLength = SquareMagnitude(); if(CurrentSquareLength > limit_length * limit_length) { float Coeff = limit_length / sqrtf(CurrentSquareLength); x *= Coeff; y *= Coeff; z *= Coeff; } } return *this; } //! Computes distance to another point inline_ float Distance(const Point& b) const { return sqrtf((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z)); } //! Computes square distance to another point inline_ float SquareDistance(const Point& b) const { return ((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z)); } //! Dot product dp = this|a inline_ float Dot(const Point& p) const { return p.x * x + p.y * y + p.z * z; } //! Cross product this = a x b inline_ Point& Cross(const Point& a, const Point& b) { x = a.y * b.z - a.z * b.y; y = a.z * b.x - a.x * b.z; z = a.x * b.y - a.y * b.x; return *this; } //! Vector code ( bitmask = sign(z) | sign(y) | sign(x) ) inline_ udword VectorCode() const { return (IR(x)>>31) | ((IR(y)&SIGN_BITMASK)>>30) | ((IR(z)&SIGN_BITMASK)>>29); } //! Returns largest axis inline_ PointComponent LargestAxis() const { const float* Vals = &x; PointComponent m = _X; if(Vals[_Y] > Vals[m]) m = _Y; if(Vals[_Z] > Vals[m]) m = _Z; return m; } //! Returns closest axis inline_ PointComponent ClosestAxis() const { const float* Vals = &x; PointComponent m = _X; if(AIR(Vals[_Y]) > AIR(Vals[m])) m = _Y; if(AIR(Vals[_Z]) > AIR(Vals[m])) m = _Z; return m; } //! Returns smallest axis inline_ PointComponent SmallestAxis() const { const float* Vals = &x; PointComponent m = _X; if(Vals[_Y] < Vals[m]) m = _Y; if(Vals[_Z] < Vals[m]) m = _Z; return m; } //! Refracts the point Point& Refract(const Point& eye, const Point& n, float refractindex, Point& refracted); //! Projects the point onto a plane Point& ProjectToPlane(const Plane& p); //! Projects the point onto the screen void ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const; //! Unfolds the point onto a plane according to edge(a,b) Point& Unfold(Plane& p, Point& a, Point& b); //! Hash function from Ville Miettinen inline_ udword GetHashValue() const { const udword* h = (const udword*)(this); udword f = (h[0]+h[1]*11-(h[2]*17)) & 0x7fffffff; // avoid problems with +-0 return (f>>22)^(f>>12)^(f); } //! Stuff magic values in the point, marking it as explicitely not used. void SetNotUsed(); //! Checks the point is marked as not used BOOL IsNotUsed() const; // Arithmetic operators //! Unary operator for Point Negate = - Point inline_ Point operator-() const { return Point(-x, -y, -z); } //! Operator for Point Plus = Point + Point. inline_ Point operator+(const Point& p) const { return Point(x + p.x, y + p.y, z + p.z); } //! Operator for Point Minus = Point - Point. inline_ Point operator-(const Point& p) const { return Point(x - p.x, y - p.y, z - p.z); } //! Operator for Point Mul = Point * Point. inline_ Point operator*(const Point& p) const { return Point(x * p.x, y * p.y, z * p.z); } //! Operator for Point Scale = Point * float. inline_ Point operator*(float s) const { return Point(x * s, y * s, z * s ); } //! Operator for Point Scale = float * Point. inline_ friend Point operator*(float s, const Point& p) { return Point(s * p.x, s * p.y, s * p.z); } //! Operator for Point Div = Point / Point. inline_ Point operator/(const Point& p) const { return Point(x / p.x, y / p.y, z / p.z); } //! Operator for Point Scale = Point / float. inline_ Point operator/(float s) const { s = 1.0f / s; return Point(x * s, y * s, z * s); } //! Operator for Point Scale = float / Point. inline_ friend Point operator/(float s, const Point& p) { return Point(s / p.x, s / p.y, s / p.z); } //! Operator for float DotProd = Point | Point. inline_ float operator|(const Point& p) const { return x*p.x + y*p.y + z*p.z; } //! Operator for Point VecProd = Point ^ Point. inline_ Point operator^(const Point& p) const { return Point( y * p.z - z * p.y, z * p.x - x * p.z, x * p.y - y * p.x ); } //! Operator for Point += Point. inline_ Point& operator+=(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; } //! Operator for Point += float. inline_ Point& operator+=(float s) { x += s; y += s; z += s; return *this; } //! Operator for Point -= Point. inline_ Point& operator-=(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; } //! Operator for Point -= float. inline_ Point& operator-=(float s) { x -= s; y -= s; z -= s; return *this; } //! Operator for Point *= Point. inline_ Point& operator*=(const Point& p) { x *= p.x; y *= p.y; z *= p.z; return *this; } //! Operator for Point *= float. inline_ Point& operator*=(float s) { x *= s; y *= s; z *= s; return *this; } //! Operator for Point /= Point. inline_ Point& operator/=(const Point& p) { x /= p.x; y /= p.y; z /= p.z; return *this; } //! Operator for Point /= float. inline_ Point& operator/=(float s) { s = 1.0f/s; x *= s; y *= s; z *= s; return *this; } // Logical operators //! Operator for "if(Point==Point)" inline_ bool operator==(const Point& p) const { return ( (IR(x)==IR(p.x))&&(IR(y)==IR(p.y))&&(IR(z)==IR(p.z))); } //! Operator for "if(Point!=Point)" inline_ bool operator!=(const Point& p) const { return ( (IR(x)!=IR(p.x))||(IR(y)!=IR(p.y))||(IR(z)!=IR(p.z))); } // Arithmetic operators //! Operator for Point Mul = Point * Matrix3x3. inline_ Point operator*(const Matrix3x3& mat) const { class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat; return Point( x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0], x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1], x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] ); } //! Operator for Point Mul = Point * Matrix4x4. inline_ Point operator*(const Matrix4x4& mat) const { class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat; return Point( x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0], x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1], x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]); } //! Operator for Point *= Matrix3x3. inline_ Point& operator*=(const Matrix3x3& mat) { class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat; float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0]; float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1]; float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2]; x = xp; y = yp; z = zp; return *this; } //! Operator for Point *= Matrix4x4. inline_ Point& operator*=(const Matrix4x4& mat) { class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat; float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0]; float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1]; float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]; x = xp; y = yp; z = zp; return *this; } // Cast operators //! Cast a Point to a HPoint. w is set to zero. operator HPoint() const; inline_ operator const float*() const { return &x; } inline_ operator float*() { return &x; } public: float x, y, z; }; FUNCTION ICEMATHS_API void Normalize1(Point& a); FUNCTION ICEMATHS_API void Normalize2(Point& a); #endif //__ICEPOINT_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IcePreprocessor.h000066400000000000000000000071241207742442300245050ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains preprocessor stuff. This should be the first included header. * \file IcePreprocessor.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICEPREPROCESSOR_H__ #define __ICEPREPROCESSOR_H__ // Check platform #if defined( _WIN32 ) || defined( WIN32 ) // #pragma message("Compiling on Windows...") #define PLATFORM_WINDOWS #else // don't issue pragmas on unknown platforms // #pragma message("Compiling on unknown platform...") #endif // Check compiler #if defined(_MSC_VER) // #pragma message("Compiling with VC++...") #define COMPILER_VISUAL_CPP #else // don't issue pragmas on unknown platforms // #pragma message("Compiling with unknown compiler...") #endif // Check compiler options. If this file is included in user-apps, this // shouldn't be needed, so that they can use what they like best. #ifndef ICE_DONT_CHECK_COMPILER_OPTIONS #ifdef COMPILER_VISUAL_CPP #if defined(_CHAR_UNSIGNED) #endif #if defined(_CPPRTTI) #error Please disable RTTI... #endif #if defined(_CPPUNWIND) #error Please disable exceptions... #endif #if defined(_MT) // Multithreading #endif #endif #endif // Check debug mode #ifdef DEBUG // May be defined instead of _DEBUG. Let's fix it. #ifndef _DEBUG #define _DEBUG #endif #endif #ifdef _DEBUG // Here you may define items for debug builds #endif #ifndef THIS_FILE #define THIS_FILE __FILE__ #endif #ifndef ICE_NO_DLL #ifdef ICECORE_EXPORTS #define ICECORE_API __declspec(dllexport) #else #define ICECORE_API __declspec(dllimport) #endif #else #define ICECORE_API #endif // Don't override new/delete // #define DEFAULT_NEWDELETE #define DONT_TRACK_MEMORY_LEAKS #define FUNCTION extern "C" // Cosmetic stuff [mainly useful with multiple inheritance] #define override(base_class) virtual // Our own inline keyword, so that: // - we can switch to __forceinline to check it's really better or not // - we can remove __forceinline if the compiler doesn't support it // #define inline_ __forceinline // #define inline_ inline // Contributed by Bruce Mitchener #if defined(COMPILER_VISUAL_CPP) #define inline_ __forceinline // #define inline_ inline #elif defined(__GNUC__) && __GNUC__ < 3 #define inline_ inline #elif defined(__GNUC__) #define inline_ inline __attribute__ ((always_inline)) #else #define inline_ inline #endif // Down the hatch #ifdef _MSC_VER #pragma inline_depth( 255 ) #endif #ifdef COMPILER_VISUAL_CPP #pragma intrinsic(memcmp) #pragma intrinsic(memcpy) #pragma intrinsic(memset) #pragma intrinsic(strcat) #pragma intrinsic(strcmp) #pragma intrinsic(strcpy) #pragma intrinsic(strlen) #pragma intrinsic(abs) #pragma intrinsic(labs) #endif // ANSI compliance #ifdef _DEBUG // Remove painful warning in debug inline_ bool __False__(){ return false; } #define for if(__False__()){} else for #else #define for if(0){} else for #endif #endif // __ICEPREPROCESSOR_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceRandom.cpp000066400000000000000000000021361207742442300235700ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for random generators. * \file IceRandom.cpp * \author Pierre Terdiman * \date August, 9, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceCore; void IceCore:: SRand(udword seed) { srand(seed); } udword IceCore::Rand() { return rand(); } static BasicRandom gRandomGenerator(42); udword IceCore::GetRandomIndex(udword max_index) { // We don't use rand() since it's limited to RAND_MAX udword Index = gRandomGenerator.Randomize(); return Index % max_index; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceRandom.h000066400000000000000000000030511207742442300232320ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for random generators. * \file IceRandom.h * \author Pierre Terdiman * \date August, 9, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICERANDOM_H__ #define __ICERANDOM_H__ FUNCTION ICECORE_API void SRand(udword seed); FUNCTION ICECORE_API udword Rand(); //! Returns a unit random floating-point value inline_ float UnitRandomFloat() { return float(Rand()) * ONE_OVER_RAND_MAX; } //! Returns a random index so that 0<= index < max_index ICECORE_API udword GetRandomIndex(udword max_index); class ICECORE_API BasicRandom { public: //! Constructor inline_ BasicRandom(udword seed=0) : mRnd(seed) {} //! Destructor inline_ ~BasicRandom() {} inline_ void SetSeed(udword seed) { mRnd = seed; } inline_ udword GetCurrentValue() const { return mRnd; } inline_ udword Randomize() { mRnd = mRnd * 2147001325 + 715136305; return mRnd; } private: udword mRnd; }; #endif // __ICERANDOM_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceRay.cpp000066400000000000000000000047001207742442300231020ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for rays. * \file IceRay.cpp * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Ray class. * A ray is a half-line P(t) = mOrig + mDir * t, with 0 <= t <= +infinity * \class Ray * \author Pierre Terdiman * \version 1.0 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* O = Origin = impact point i = normalized vector along the x axis j = normalized vector along the y axis = actually the normal vector in O D = Direction vector, norm |D| = 1 N = Projection of D on y axis, norm |N| = normal reaction T = Projection of D on x axis, norm |T| = tangential reaction R = Reflexion vector ^y | | | _ _ _| _ _ _ * * *| \ | / \ |N / | R\ | /D \ | / | \ | / _________\|/______*_______>x O T Let define theta = angle between D and N. Then cos(theta) = |N| / |D| = |N| since D is normalized. j|D = |j|*|D|*cos(theta) => |N| = j|D Then we simply have: D = N + T To compute tangential reaction : T = D - N To compute reflexion vector : R = N - T = N - (D-N) = 2*N - D */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; float Ray::SquareDistance(const Point& point, float* t) const { Point Diff = point - mOrig; float fT = Diff | mDir; if(fT<=0.0f) { fT = 0.0f; } else { fT /= mDir.SquareMagnitude(); Diff -= fT*mDir; } if(t) *t = fT; return Diff.SquareMagnitude(); } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceRay.h000066400000000000000000000111201207742442300225410ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for rays. * \file IceRay.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICERAY_H__ #define __ICERAY_H__ class ICEMATHS_API Ray { public: //! Constructor inline_ Ray() {} //! Constructor inline_ Ray(const Point& orig, const Point& dir) : mOrig(orig), mDir(dir) {} //! Copy constructor inline_ Ray(const Ray& ray) : mOrig(ray.mOrig), mDir(ray.mDir) {} //! Destructor inline_ ~Ray() {} float SquareDistance(const Point& point, float* t=null) const; inline_ float Distance(const Point& point, float* t=null) const { return sqrtf(SquareDistance(point, t)); } Point mOrig; //!< Ray origin Point mDir; //!< Normalized direction }; inline_ void ComputeReflexionVector(Point& reflected, const Point& incoming_dir, const Point& outward_normal) { reflected = incoming_dir - outward_normal * 2.0f * (incoming_dir|outward_normal); } inline_ void ComputeReflexionVector(Point& reflected, const Point& source, const Point& impact, const Point& normal) { Point V = impact - source; reflected = V - normal * 2.0f * (V|normal); } inline_ void DecomposeVector(Point& normal_compo, Point& tangent_compo, const Point& outward_dir, const Point& outward_normal) { normal_compo = outward_normal * (outward_dir|outward_normal); tangent_compo = outward_dir - normal_compo; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Transforms a direction vector from world space to local space * \param local_dir [out] direction vector in local space * \param world_dir [in] direction vector in world space * \param world [in] world transform */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void ComputeLocalDirection(Point& local_dir, const Point& world_dir, const Matrix4x4& world) { // Get world direction back in local space // Matrix3x3 InvWorld = world; // local_dir = InvWorld * world_dir; local_dir = Matrix3x3(world) * world_dir; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Transforms a position vector from world space to local space * \param local_pt [out] position vector in local space * \param world_pt [in] position vector in world space * \param world [in] world transform */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void ComputeLocalPoint(Point& local_pt, const Point& world_pt, const Matrix4x4& world) { // Get world vertex back in local space Matrix4x4 InvWorld = world; InvWorld.Invert(); local_pt = world_pt * InvWorld; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Transforms a ray from world space to local space * \param local_ray [out] ray in local space * \param world_ray [in] ray in world space * \param world [in] world transform */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void ComputeLocalRay(Ray& local_ray, const Ray& world_ray, const Matrix4x4& world) { // Get world ray back in local space ComputeLocalDirection(local_ray.mDir, world_ray.mDir, world); ComputeLocalPoint(local_ray.mOrig, world_ray.mOrig, world); } #endif // __ICERAY_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceRevisitedRadix.cpp000066400000000000000000000530021207742442300252740ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains source code from the article "Radix Sort Revisited". * \file IceRevisitedRadix.cpp * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Revisited Radix Sort. * This is my new radix routine: * - it uses indices and doesn't recopy the values anymore, hence wasting less ram * - it creates all the histograms in one run instead of four * - it sorts words faster than dwords and bytes faster than words * - it correctly sorts negative floating-point values by patching the offsets * - it automatically takes advantage of temporal coherence * - multiple keys support is a side effect of temporal coherence * - it may be worth recoding in asm... (mainly to use FCOMI, FCMOV, etc) [it's probably memory-bound anyway] * * History: * - 08.15.98: very first version * - 04.04.00: recoded for the radix article * - 12.xx.00: code lifting * - 09.18.01: faster CHECK_PASS_VALIDITY thanks to Mark D. Shattuck (who provided other tips, not included here) * - 10.11.01: added local ram support * - 01.20.02: bugfix! In very particular cases the last pass was skipped in the float code-path, leading to incorrect sorting...... * - 01.02.02: - "mIndices" renamed => "mRanks". That's a rank sorter after all. * - ranks are not "reset" anymore, but implicit on first calls * - 07.05.02: - offsets rewritten with one less indirection. * - 11.03.02: - "bool" replaced with RadixHint enum * * \class RadixSort * \author Pierre Terdiman * \version 1.4 * \date August, 15, 1998 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* To do: - add an offset parameter between two input values (avoid some data recopy sometimes) - unroll ? asm ? - 11 bits trick & 3 passes as Michael did - prefetch stuff the day I have a P3 - make a version with 16-bits indices ? */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceCore; #define INVALIDATE_RANKS mCurrentSize|=0x80000000 #define VALIDATE_RANKS mCurrentSize&=0x7fffffff #define CURRENT_SIZE (mCurrentSize&0x7fffffff) #define INVALID_RANKS (mCurrentSize&0x80000000) #define CHECK_RESIZE(n) \ if(n!=mPreviousSize) \ { \ if(n>mCurrentSize) Resize(n); \ else ResetRanks(); \ mPreviousSize = n; \ } #define CREATE_HISTOGRAMS(type, buffer) \ /* Clear counters/histograms */ \ ZeroMemory(mHistogram, 256*4*sizeof(udword)); \ \ /* Prepare to count */ \ ubyte* p = (ubyte*)input; \ ubyte* pe = &p[nb*4]; \ udword* h0= &mHistogram[0]; /* Histogram for first pass (LSB) */ \ udword* h1= &mHistogram[256]; /* Histogram for second pass */ \ udword* h2= &mHistogram[512]; /* Histogram for third pass */ \ udword* h3= &mHistogram[768]; /* Histogram for last pass (MSB) */ \ \ bool AlreadySorted = true; /* Optimism... */ \ \ if(INVALID_RANKS) \ { \ /* Prepare for temporal coherence */ \ type* Running = (type*)buffer; \ type PrevVal = *Running; \ \ while(p!=pe) \ { \ /* Read input buffer in previous sorted order */ \ type Val = *Running++; \ /* Check whether already sorted or not */ \ if(ValCurSize) Resize(nb); mCurrentSize = nb; INVALIDATE_RANKS; } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Main sort routine. * This one is for integer values. After the call, mRanks contains a list of indices in sorted order, i.e. in the order you may process your data. * \param input [in] a list of integer values to sort * \param nb [in] number of values to sort, must be < 2^31 * \param hint [in] RADIX_SIGNED to handle negative values, RADIX_UNSIGNED if you know your input buffer only contains positive values * \return Self-Reference */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// RadixSort& RadixSort::Sort(const udword* input, udword nb, RadixHint hint) { // Checkings if(!input || !nb || nb&0x80000000) return *this; // Stats mTotalCalls++; // Resize lists if needed CheckResize(nb); #ifdef RADIX_LOCAL_RAM // Allocate histograms & offsets on the stack udword mHistogram[256*4]; // udword mOffset[256]; udword* mLink[256]; #endif // Create histograms (counters). Counters for all passes are created in one run. // Pros: read input buffer once instead of four times // Cons: mHistogram is 4Kb instead of 1Kb // We must take care of signed/unsigned values for temporal coherence.... I just // have 2 code paths even if just a single opcode changes. Self-modifying code, someone? if(hint==RADIX_UNSIGNED) { CREATE_HISTOGRAMS(udword, input); } else { CREATE_HISTOGRAMS(sdword, input); } // Compute #negative values involved if needed udword NbNegativeValues = 0; if(hint==RADIX_SIGNED) { // An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128 // last values of the last histogram. Last histogram because that's the one for the Most Significant Byte, // responsible for the sign. 128 last values because the 128 first ones are related to positive numbers. udword* h3= &mHistogram[768]; for(udword i=128;i<256;i++) NbNegativeValues += h3[i]; // 768 for last histogram, 128 for negative part } // Radix sort, j is the pass number (0=LSB, 3=MSB) for(udword j=0;j<4;j++) { CHECK_PASS_VALIDITY(j); // Sometimes the fourth (negative) pass is skipped because all numbers are negative and the MSB is 0xFF (for example). This is // not a problem, numbers are correctly sorted anyway. if(PerformPass) { // Should we care about negative values? if(j!=3 || hint==RADIX_UNSIGNED) { // Here we deal with positive values only // Create offsets // mOffset[0] = 0; // for(udword i=1;i<256;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; mLink[0] = mRanks2; for(udword i=1;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; } else { // This is a special case to correctly handle negative integers. They're sorted in the right order but at the wrong place. // Create biased offsets, in order for negative numbers to be sorted as well // mOffset[0] = NbNegativeValues; // First positive number takes place after the negative ones mLink[0] = &mRanks2[NbNegativeValues]; // First positive number takes place after the negative ones // for(udword i=1;i<128;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers for(udword i=1;i<128;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers // Fixing the wrong place for negative values // mOffset[128] = 0; mLink[128] = mRanks2; // for(i=129;i<256;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; for(udword i=129;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; } // Perform Radix Sort ubyte* InputBytes = (ubyte*)input; InputBytes += j; if(INVALID_RANKS) { // for(udword i=0;i>24; // Radix byte, same as above. AND is useless here (udword). // ### cmp to be killed. Not good. Later. // if(Radix<128) mRanks2[mOffset[Radix]++] = i; // Number is positive, same as above // else mRanks2[--mOffset[Radix]] = i; // Number is negative, flip the sorting order if(Radix<128) *mLink[Radix]++ = i; // Number is positive, same as above else *(--mLink[Radix]) = i; // Number is negative, flip the sorting order } VALIDATE_RANKS; } else { for(udword i=0;i>24; // Radix byte, same as above. AND is useless here (udword). // ### cmp to be killed. Not good. Later. // if(Radix<128) mRanks2[mOffset[Radix]++] = mRanks[i]; // Number is positive, same as above // else mRanks2[--mOffset[Radix]] = mRanks[i]; // Number is negative, flip the sorting order if(Radix<128) *mLink[Radix]++ = mRanks[i]; // Number is positive, same as above else *(--mLink[Radix]) = mRanks[i]; // Number is negative, flip the sorting order } } // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap. udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp; } else { // The pass is useless, yet we still have to reverse the order of current list if all values are negative. if(UniqueVal>=128) { if(INVALID_RANKS) { // ###Possible? for(udword i=0;i=SqrLen) { fT = 1.0f; Diff -= Dir; } else { fT /= SqrLen; Diff -= fT*Dir; } } if(t) *t = fT; return Diff.SquareMagnitude(); } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceSegment.h000066400000000000000000000051021207742442300234130ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for segments. * \file IceSegment.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICESEGMENT_H__ #define __ICESEGMENT_H__ class ICEMATHS_API Segment { public: //! Constructor inline_ Segment() {} //! Constructor inline_ Segment(const Point& p0, const Point& p1) : mP0(p0), mP1(p1) {} //! Copy constructor inline_ Segment(const Segment& seg) : mP0(seg.mP0), mP1(seg.mP1) {} //! Destructor inline_ ~Segment() {} inline_ const Point& GetOrigin() const { return mP0; } inline_ Point ComputeDirection() const { return mP1 - mP0; } inline_ void ComputeDirection(Point& dir) const { dir = mP1 - mP0; } inline_ float ComputeLength() const { return mP1.Distance(mP0); } inline_ float ComputeSquareLength() const { return mP1.SquareDistance(mP0); } inline_ void SetOriginDirection(const Point& origin, const Point& direction) { mP0 = mP1 = origin; mP1 += direction; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes a point on the segment * \param pt [out] point on segment * \param t [in] point's parameter [t=0 => pt = mP0, t=1 => pt = mP1] */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void ComputePoint(Point& pt, float t) const { pt = mP0 + t * (mP1 - mP0); } float SquareDistance(const Point& point, float* t=null) const; inline_ float Distance(const Point& point, float* t=null) const { return sqrtf(SquareDistance(point, t)); } Point mP0; //!< Start of segment Point mP1; //!< End of segment }; #endif // __ICESEGMENT_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceTriList.h000066400000000000000000000042431207742442300234100ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for a triangle container. * \file IceTrilist.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICETRILIST_H__ #define __ICETRILIST_H__ class ICEMATHS_API TriList : public Container { public: // Constructor / Destructor TriList() {} ~TriList() {} inline_ udword GetNbTriangles() const { return GetNbEntries()/9; } inline_ Triangle* GetTriangles() const { return (Triangle*)GetEntries(); } void AddTri(const Triangle& tri) { Add(tri.mVerts[0].x).Add(tri.mVerts[0].y).Add(tri.mVerts[0].z); Add(tri.mVerts[1].x).Add(tri.mVerts[1].y).Add(tri.mVerts[1].z); Add(tri.mVerts[2].x).Add(tri.mVerts[2].y).Add(tri.mVerts[2].z); } void AddTri(const Point& p0, const Point& p1, const Point& p2) { Add(p0.x).Add(p0.y).Add(p0.z); Add(p1.x).Add(p1.y).Add(p1.z); Add(p2.x).Add(p2.y).Add(p2.z); } }; class ICEMATHS_API TriangleList : public Container { public: // Constructor / Destructor TriangleList() {} ~TriangleList() {} inline_ udword GetNbTriangles() const { return GetNbEntries()/3; } inline_ IndexedTriangle* GetTriangles() const { return (IndexedTriangle*)GetEntries();} void AddTriangle(const IndexedTriangle& tri) { Add(tri.mVRef[0]).Add(tri.mVRef[1]).Add(tri.mVRef[2]); } void AddTriangle(udword vref0, udword vref1, udword vref2) { Add(vref0).Add(vref1).Add(vref2); } }; #endif //__ICETRILIST_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceTriangle.cpp000066400000000000000000000301521207742442300241140ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a handy triangle class. * \file IceTriangle.cpp * \author Pierre Terdiman * \date January, 17, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace IceMaths; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a triangle class. * * \class Tri * \author Pierre Terdiman * \version 1.0 * \date 08.15.98 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// static sdword VPlaneSideEps(const Point& v, const Plane& plane, float epsilon) { // Compute distance from current vertex to the plane float Dist = plane.Distance(v); // Compute side: // 1 = the vertex is on the positive side of the plane // -1 = the vertex is on the negative side of the plane // 0 = the vertex is on the plane (within epsilon) return Dist > epsilon ? 1 : Dist < -epsilon ? -1 : 0; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Flips the winding order. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Triangle::Flip() { Point Tmp = mVerts[1]; mVerts[1] = mVerts[2]; mVerts[2] = Tmp; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle area. * \return the area */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float Triangle::Area() const { const Point& p0 = mVerts[0]; const Point& p1 = mVerts[1]; const Point& p2 = mVerts[2]; return ((p0 - p1)^(p0 - p2)).Magnitude() * 0.5f; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle perimeter. * \return the perimeter */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float Triangle::Perimeter() const { const Point& p0 = mVerts[0]; const Point& p1 = mVerts[1]; const Point& p2 = mVerts[2]; return p0.Distance(p1) + p0.Distance(p2) + p1.Distance(p2); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle compacity. * \return the compacity */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float Triangle::Compacity() const { float P = Perimeter(); if(P==0.0f) return 0.0f; return (4.0f*PI*Area()/(P*P)); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle normal. * \param normal [out] the computed normal */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Triangle::Normal(Point& normal) const { const Point& p0 = mVerts[0]; const Point& p1 = mVerts[1]; const Point& p2 = mVerts[2]; normal = ((p0 - p1)^(p0 - p2)).Normalize(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle denormalized normal. * \param normal [out] the computed normal */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Triangle::DenormalizedNormal(Point& normal) const { const Point& p0 = mVerts[0]; const Point& p1 = mVerts[1]; const Point& p2 = mVerts[2]; normal = ((p0 - p1)^(p0 - p2)); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle center. * \param center [out] the computed center */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Triangle::Center(Point& center) const { const Point& p0 = mVerts[0]; const Point& p1 = mVerts[1]; const Point& p2 = mVerts[2]; center = (p0 + p1 + p2)*INV3; } PartVal Triangle::TestAgainstPlane(const Plane& plane, float epsilon) const { bool Pos = false, Neg = false; // Loop through all vertices for(udword i=0;i<3;i++) { // Compute side: sdword Side = VPlaneSideEps(mVerts[i], plane, epsilon); if (Side < 0) Neg = true; else if (Side > 0) Pos = true; } if (!Pos && !Neg) return TRI_ON_PLANE; else if (Pos && Neg) return TRI_INTERSECT; else if (Pos && !Neg) return TRI_PLUS_SPACE; else if (!Pos && Neg) return TRI_MINUS_SPACE; // What?! return TRI_FORCEDWORD; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle moment. * \param m [out] the moment */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* void Triangle::ComputeMoment(Moment& m) { // Compute the area of the triangle m.mArea = Area(); // Compute the centroid Center(m.mCentroid); // Second-order components. Handle zero-area faces. Point& p = mVerts[0]; Point& q = mVerts[1]; Point& r = mVerts[2]; if(m.mArea==0.0f) { // This triangle has zero area. The second order components would be eliminated with the usual formula, so, for the // sake of robustness we use an alternative form. These are the centroid and second-order components of the triangle's vertices. m.mCovariance.m[0][0] = (p.x*p.x + q.x*q.x + r.x*r.x); m.mCovariance.m[0][1] = (p.x*p.y + q.x*q.y + r.x*r.y); m.mCovariance.m[0][2] = (p.x*p.z + q.x*q.z + r.x*r.z); m.mCovariance.m[1][1] = (p.y*p.y + q.y*q.y + r.y*r.y); m.mCovariance.m[1][2] = (p.y*p.z + q.y*q.z + r.y*r.z); m.mCovariance.m[2][2] = (p.z*p.z + q.z*q.z + r.z*r.z); m.mCovariance.m[2][1] = m.mCovariance.m[1][2]; m.mCovariance.m[1][0] = m.mCovariance.m[0][1]; m.mCovariance.m[2][0] = m.mCovariance.m[0][2]; } else { const float OneOverTwelve = 1.0f / 12.0f; m.mCovariance.m[0][0] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.x + p.x*p.x + q.x*q.x + r.x*r.x) * OneOverTwelve; m.mCovariance.m[0][1] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.y + p.x*p.y + q.x*q.y + r.x*r.y) * OneOverTwelve; m.mCovariance.m[1][1] = m.mArea * (9.0f * m.mCentroid.y*m.mCentroid.y + p.y*p.y + q.y*q.y + r.y*r.y) * OneOverTwelve; m.mCovariance.m[0][2] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.z + p.x*p.z + q.x*q.z + r.x*r.z) * OneOverTwelve; m.mCovariance.m[1][2] = m.mArea * (9.0f * m.mCentroid.y*m.mCentroid.z + p.y*p.z + q.y*q.z + r.y*r.z) * OneOverTwelve; m.mCovariance.m[2][2] = m.mArea * (9.0f * m.mCentroid.z*m.mCentroid.z + p.z*p.z + q.z*q.z + r.z*r.z) * OneOverTwelve; m.mCovariance.m[2][1] = m.mCovariance.m[1][2]; m.mCovariance.m[1][0] = m.mCovariance.m[0][1]; m.mCovariance.m[2][0] = m.mCovariance.m[0][2]; } } */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle's smallest edge length. * \return the smallest edge length */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float Triangle::MinEdgeLength() const { float Min = MAX_FLOAT; float Length01 = mVerts[0].Distance(mVerts[1]); float Length02 = mVerts[0].Distance(mVerts[2]); float Length12 = mVerts[1].Distance(mVerts[2]); if(Length01 < Min) Min = Length01; if(Length02 < Min) Min = Length02; if(Length12 < Min) Min = Length12; return Min; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the triangle's largest edge length. * \return the largest edge length */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float Triangle::MaxEdgeLength() const { float Max = MIN_FLOAT; float Length01 = mVerts[0].Distance(mVerts[1]); float Length02 = mVerts[0].Distance(mVerts[2]); float Length12 = mVerts[1].Distance(mVerts[2]); if(Length01 > Max) Max = Length01; if(Length02 > Max) Max = Length02; if(Length12 > Max) Max = Length12; return Max; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes a point on the triangle according to the stabbing information. * \param u,v [in] point's barycentric coordinates * \param pt [out] point on triangle * \param nearvtx [out] index of nearest vertex */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Triangle::ComputePoint(float u, float v, Point& pt, udword* nearvtx) const { // Compute point coordinates pt = (1.0f - u - v)*mVerts[0] + u*mVerts[1] + v*mVerts[2]; // Compute nearest vertex if needed if(nearvtx) { // Compute distance vector Point d(mVerts[0].SquareDistance(pt), // Distance^2 from vertex 0 to point on the face mVerts[1].SquareDistance(pt), // Distance^2 from vertex 1 to point on the face mVerts[2].SquareDistance(pt)); // Distance^2 from vertex 2 to point on the face // Get smallest distance *nearvtx = d.SmallestAxis(); } } void Triangle::Inflate(float fat_coeff, bool constant_border) { // Compute triangle center Point TriangleCenter; Center(TriangleCenter); // Don't normalize? // Normalize => add a constant border, regardless of triangle size // Don't => add more to big triangles for(udword i=0;i<3;i++) { Point v = mVerts[i] - TriangleCenter; if(constant_border) v.Normalize(); mVerts[i] += v * fat_coeff; } } choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceTriangle.h000066400000000000000000000046641207742442300235720ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a handy triangle class. * \file IceTriangle.h * \author Pierre Terdiman * \date January, 17, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICETRIANGLE_H__ #define __ICETRIANGLE_H__ // Forward declarations class Moment; // Partitioning values enum PartVal { TRI_MINUS_SPACE = 0, //!< Triangle is in the negative space TRI_PLUS_SPACE = 1, //!< Triangle is in the positive space TRI_INTERSECT = 2, //!< Triangle intersects plane TRI_ON_PLANE = 3, //!< Triangle and plane are coplanar TRI_FORCEDWORD = 0x7fffffff }; // A triangle class. class ICEMATHS_API Triangle { public: //! Constructor inline_ Triangle() {} //! Constructor inline_ Triangle(const Point& p0, const Point& p1, const Point& p2) { mVerts[0]=p0; mVerts[1]=p1; mVerts[2]=p2; } //! Copy constructor inline_ Triangle(const Triangle& triangle) { mVerts[0] = triangle.mVerts[0]; mVerts[1] = triangle.mVerts[1]; mVerts[2] = triangle.mVerts[2]; } //! Destructor inline_ ~Triangle() {} //! Vertices Point mVerts[3]; // Methods void Flip(); float Area() const; float Perimeter() const; float Compacity() const; void Normal(Point& normal) const; void DenormalizedNormal(Point& normal) const; void Center(Point& center) const; inline_ Plane PlaneEquation() const { return Plane(mVerts[0], mVerts[1], mVerts[2]); } PartVal TestAgainstPlane(const Plane& plane, float epsilon) const; // float Distance(Point& cp, Point& cq, Tri& tri); void ComputeMoment(Moment& m); float MinEdgeLength() const; float MaxEdgeLength() const; void ComputePoint(float u, float v, Point& pt, udword* nearvtx=null) const; void Inflate(float fat_coeff, bool constant_border); }; #endif // __ICETRIANGLE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Ice/IceTypes.h000066400000000000000000000170741207742442300231300ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains custom types. * \file IceTypes.h * \author Pierre Terdiman * \date April, 4, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __ICETYPES_H__ #define __ICETYPES_H__ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Things to help us compile on non-windows platforms #if 0 #if defined(__MACOSX__) || defined(__APPLE__) #undef bool #define bool char #undef true #define true ((bool)-1) #undef false #define false ((bool)0) #endif // mac stuff #endif /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// #define USE_HANDLE_MANAGER // Constants #define PI 3.1415926535897932384626433832795028841971693993751f //!< PI #define HALFPI 1.57079632679489661923f //!< 0.5 * PI #define TWOPI 6.28318530717958647692f //!< 2.0 * PI #define INVPI 0.31830988618379067154f //!< 1.0 / PI #define RADTODEG 57.2957795130823208768f //!< 180.0 / PI, convert radians to degrees #define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians #define EXP 2.71828182845904523536f //!< e #define INVLOG2 3.32192809488736234787f //!< 1.0 / log10(2) #define LN2 0.693147180559945f //!< ln(2) #define INVLN2 1.44269504089f //!< 1.0f / ln(2) #define INV3 0.33333333333333333333f //!< 1/3 #define INV6 0.16666666666666666666f //!< 1/6 #define INV7 0.14285714285714285714f //!< 1/7 #define INV9 0.11111111111111111111f //!< 1/9 #define INV255 0.00392156862745098039f //!< 1/255 #define SQRT2 1.41421356237f //!< sqrt(2) #define INVSQRT2 0.707106781188f //!< 1 / sqrt(2) #define SQRT3 1.73205080757f //!< sqrt(3) #define INVSQRT3 0.577350269189f //!< 1 / sqrt(3) #define null 0 //!< our own NULL pointer // Custom types used in ICE typedef signed char sbyte; //!< sizeof(sbyte) must be 1 typedef unsigned char ubyte; //!< sizeof(ubyte) must be 1 typedef signed short sword; //!< sizeof(sword) must be 2 typedef unsigned short uword; //!< sizeof(uword) must be 2 typedef signed int sdword; //!< sizeof(sdword) must be 4 typedef unsigned int udword; //!< sizeof(udword) must be 4 typedef signed __int64 sqword; //!< sizeof(sqword) must be 8 typedef unsigned __int64 uqword; //!< sizeof(uqword) must be 8 typedef float float32; //!< sizeof(float32) must be 4 typedef double float64; //!< sizeof(float64) must be 4 ICE_COMPILE_TIME_ASSERT(sizeof(bool)==1); // ...otherwise things might fail with VC++ 4.2 ! ICE_COMPILE_TIME_ASSERT(sizeof(ubyte)==1); ICE_COMPILE_TIME_ASSERT(sizeof(sbyte)==1); ICE_COMPILE_TIME_ASSERT(sizeof(sword)==2); ICE_COMPILE_TIME_ASSERT(sizeof(uword)==2); ICE_COMPILE_TIME_ASSERT(sizeof(udword)==4); ICE_COMPILE_TIME_ASSERT(sizeof(sdword)==4); ICE_COMPILE_TIME_ASSERT(sizeof(uqword)==8); ICE_COMPILE_TIME_ASSERT(sizeof(sqword)==8); //! TO BE DOCUMENTED #define DECLARE_ICE_HANDLE(name) struct name##__ { int unused; }; typedef struct name##__ *name typedef udword DynID; //!< Dynamic identifier #ifdef USE_HANDLE_MANAGER typedef udword KID; //!< Kernel ID // DECLARE_ICE_HANDLE(KID); #else typedef uword KID; //!< Kernel ID #endif typedef udword RTYPE; //!< Relationship-type (!) between owners and references #define INVALID_ID 0xffffffff //!< Invalid dword ID (counterpart of null pointers) #ifdef USE_HANDLE_MANAGER #define INVALID_KID 0xffffffff //!< Invalid Kernel ID #else #define INVALID_KID 0xffff //!< Invalid Kernel ID #endif #define INVALID_NUMBER 0xDEADBEEF //!< Standard junk value // Define BOOL if needed #ifndef BOOL typedef int BOOL; //!< Another boolean type. #endif //! Union of a float and a sdword typedef union { float f; //!< The float sdword d; //!< The integer }scell; //! Union of a float and a udword typedef union { float f; //!< The float udword d; //!< The integer }ucell; // Type ranges #define MAX_SBYTE 0x7f //!< max possible sbyte value #define MIN_SBYTE 0x80 //!< min possible sbyte value #define MAX_UBYTE 0xff //!< max possible ubyte value #define MIN_UBYTE 0x00 //!< min possible ubyte value #define MAX_SWORD 0x7fff //!< max possible sword value #define MIN_SWORD 0x8000 //!< min possible sword value #define MAX_UWORD 0xffff //!< max possible uword value #define MIN_UWORD 0x0000 //!< min possible uword value #define MAX_SDWORD 0x7fffffff //!< max possible sdword value #define MIN_SDWORD 0x80000000 //!< min possible sdword value #define MAX_UDWORD 0xffffffff //!< max possible udword value #define MIN_UDWORD 0x00000000 //!< min possible udword value #define MAX_FLOAT FLT_MAX //!< max possible float value #define MIN_FLOAT (-FLT_MAX) //!< min possible loat value #define IEEE_1_0 0x3f800000 //!< integer representation of 1.0 #define IEEE_255_0 0x437f0000 //!< integer representation of 255.0 #define IEEE_MAX_FLOAT 0x7f7fffff //!< integer representation of MAX_FLOAT #define IEEE_MIN_FLOAT 0xff7fffff //!< integer representation of MIN_FLOAT #define IEEE_UNDERFLOW_LIMIT 0x1a000000 #define ONE_OVER_RAND_MAX (1.0f / float(RAND_MAX)) //!< Inverse of the max possible value returned by rand() typedef int (__stdcall* PROC)(); //!< A standard procedure call. typedef bool (*ENUMERATION)(udword value, udword param, udword context); //!< ICE standard enumeration call typedef void** VTABLE; //!< A V-Table. #undef MIN #undef MAX #define MIN(a, b) ((a) < (b) ? (a) : (b)) //!< Returns the min value between a and b #define MAX(a, b) ((a) > (b) ? (a) : (b)) //!< Returns the max value between a and b #define MAXMAX(a,b,c) ((a) > (b) ? MAX (a,c) : MAX (b,c)) //!< Returns the max value between a, b and c template inline_ const T& TMin (const T& a, const T& b) { return b < a ? b : a; } template inline_ const T& TMax (const T& a, const T& b) { return a < b ? b : a; } template inline_ void TSetMin (T& a, const T& b) { if(a>b) a = b; } template inline_ void TSetMax (T& a, const T& b) { if(a> 1) & 0x55555555) | ((n << 1) & 0xaaaaaaaa); n = ((n >> 2) & 0x33333333) | ((n << 2) & 0xcccccccc); n = ((n >> 4) & 0x0f0f0f0f) | ((n << 4) & 0xf0f0f0f0); n = ((n >> 8) & 0x00ff00ff) | ((n << 8) & 0xff00ff00); n = ((n >> 16) & 0x0000ffff) | ((n << 16) & 0xffff0000); // Etc for larger intergers (64 bits in Java) // NOTE: the >> operation must be unsigned! (>>> in java) } //! Count the number of '1' bits in a 32 bit word (from Steve Baker's Cute Code Collection) inline_ udword CountBits(udword n) { // This relies of the fact that the count of n bits can NOT overflow // an n bit interger. EG: 1 bit count takes a 1 bit interger, 2 bit counts // 2 bit interger, 3 bit count requires only a 2 bit interger. // So we add all bit pairs, then each nible, then each byte etc... n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1); n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2); n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4); n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8); n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16); // Etc for larger intergers (64 bits in Java) // NOTE: the >> operation must be unsigned! (>>> in java) return n; } //! Even faster? inline_ udword CountBits2(udword bits) { bits = bits - ((bits >> 1) & 0x55555555); bits = ((bits >> 2) & 0x33333333) + (bits & 0x33333333); bits = ((bits >> 4) + bits) & 0x0F0F0F0F; return (bits * 0x01010101) >> 24; } //! Spread out bits. EG 00001111 -> 0101010101 //! 00001010 -> 0100010000 //! This is used to interleve to intergers to produce a `Morten Key' //! used in Space Filling Curves (See DrDobbs Journal, July 1999) //! Order is important. inline_ void SpreadBits(udword& n) { n = ( n & 0x0000ffff) | (( n & 0xffff0000) << 16); n = ( n & 0x000000ff) | (( n & 0x0000ff00) << 8); n = ( n & 0x000f000f) | (( n & 0x00f000f0) << 4); n = ( n & 0x03030303) | (( n & 0x0c0c0c0c) << 2); n = ( n & 0x11111111) | (( n & 0x22222222) << 1); } // Next Largest Power of 2 // Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm // that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with // the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next // largest power of 2. For a 32-bit value: inline_ udword nlpo2(udword x) { x |= (x >> 1); x |= (x >> 2); x |= (x >> 4); x |= (x >> 8); x |= (x >> 16); return x+1; } //! Test to see if a number is an exact power of two (from Steve Baker's Cute Code Collection) inline_ bool IsPowerOfTwo(udword n) { return ((n&(n-1))==0); } //! Zero the least significant '1' bit in a word. (from Steve Baker's Cute Code Collection) inline_ void ZeroLeastSetBit(udword& n) { n&=(n-1); } //! Set the least significant N bits in a word. (from Steve Baker's Cute Code Collection) inline_ void SetLeastNBits(udword& x, udword n) { x|=~(~0<> 31; return (x^y)-y; } //!< Alternative min function inline_ sdword min_(sdword a, sdword b) { sdword delta = b-a; return a + (delta&(delta>>31)); } // Determine if one of the bytes in a 4 byte word is zero inline_ BOOL HasNullByte(udword x) { return ((x + 0xfefefeff) & (~x) & 0x80808080); } // To find the smallest 1 bit in a word EG: ~~~~~~10---0 => 0----010---0 inline_ udword LowestOneBit(udword w) { return ((w) & (~(w)+1)); } // inline_ udword LowestOneBit_(udword w) { return ((w) & (-(w))); } // Most Significant 1 Bit // Given a binary integer value x, the most significant 1 bit (highest numbered element of a bit set) // can be computed using a SWAR algorithm that recursively "folds" the upper bits into the lower bits. // This process yields a bit vector with the same most significant 1 as x, but all 1's below it. // Bitwise AND of the original value with the complement of the "folded" value shifted down by one // yields the most significant bit. For a 32-bit value: inline_ udword msb32(udword x) { x |= (x >> 1); x |= (x >> 2); x |= (x >> 4); x |= (x >> 8); x |= (x >> 16); return (x & ~(x >> 1)); } /* "Just call it repeatedly with various input values and always with the same variable as "memory". The sharpness determines the degree of filtering, where 0 completely filters out the input, and 1 does no filtering at all. I seem to recall from college that this is called an IIR (Infinite Impulse Response) filter. As opposed to the more typical FIR (Finite Impulse Response). Also, I'd say that you can make more intelligent and interesting filters than this, for example filters that remove wrong responses from the mouse because it's being moved too fast. You'd want such a filter to be applied before this one, of course." (JCAB on Flipcode) */ inline_ float FeedbackFilter(float val, float& memory, float sharpness) { ASSERT(sharpness>=0.0f && sharpness<=1.0f && "Invalid sharpness value in feedback filter"); if(sharpness<0.0f) sharpness = 0.0f; else if(sharpness>1.0f) sharpness = 1.0f; return memory = val * sharpness + memory * (1.0f - sharpness); } //! If you can guarantee that your input domain (i.e. value of x) is slightly //! limited (abs(x) must be < ((1<<31u)-32767)), then you can use the //! following code to clamp the resulting value into [-32768,+32767] range: inline_ int ClampToInt16(int x) { // ASSERT(abs(x) < (int)((1<<31u)-32767)); int delta = 32767 - x; x += (delta>>31) & delta; delta = x + 32768; x -= (delta>>31) & delta; return x; } // Generic functions template inline_ void TSwap(Type& a, Type& b) { const Type c = a; a = b; b = c; } template inline_ Type TClamp(const Type& x, const Type& lo, const Type& hi) { return ((xhi) ? hi : x); } template inline_ void TSort(Type& a, Type& b) { if(a>b) TSwap(a, b); } template inline_ void TSort(Type& a, Type& b, Type& c) { if(a>b) TSwap(a, b); if(b>c) TSwap(b, c); if(a>b) TSwap(a, b); if(b>c) TSwap(b, c); } // Prevent nasty user-manipulations (strategy borrowed from Charles Bloom) // #define PREVENT_COPY(curclass) void operator = (const curclass& object) { ASSERT(!"Bad use of operator ="); } // ... actually this is better ! #define PREVENT_COPY(cur_class) private: cur_class(const cur_class& object); cur_class& operator=(const cur_class& object); //! TO BE DOCUMENTED #define OFFSET_OF(Class, Member) (size_t)&(((Class*)0)->Member) //! TO BE DOCUMENTED #define ARRAYSIZE(p) (sizeof(p)/sizeof(p[0])) /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Returns the alignment of the input address. * \fn Alignment() * \param address [in] address to check * \return the best alignment (e.g. 1 for odd addresses, etc) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// FUNCTION ICECORE_API udword Alignment(udword address); #define IS_ALIGNED_2(x) ((x&1)==0) #define IS_ALIGNED_4(x) ((x&3)==0) #define IS_ALIGNED_8(x) ((x&7)==0) inline_ void _prefetch(void const* ptr) { (void)*(char const volatile *)ptr; } // Compute implicit coords from an index: // The idea is to get back 2D coords from a 1D index. // For example: // // 0 1 2 ... nbu-1 // nbu nbu+1 i ... // // We have i, we're looking for the equivalent (u=2, v=1) location. // i = u + v*nbu // <=> i/nbu = u/nbu + v // Since 0 <= u < nbu, u/nbu = 0 (integer) // Hence: v = i/nbu // Then we simply put it back in the original equation to compute u = i - v*nbu inline_ void Compute2DCoords(udword& u, udword& v, udword i, udword nbu) { v = i / nbu; u = i - (v * nbu); } // In 3D: i = u + v*nbu + w*nbu*nbv // <=> i/(nbu*nbv) = u/(nbu*nbv) + v/nbv + w // u/(nbu*nbv) is null since u/nbu was null already. // v/nbv is null as well for the same reason. // Hence w = i/(nbu*nbv) // Then we're left with a 2D problem: i' = i - w*nbu*nbv = u + v*nbu inline_ void Compute3DCoords(udword& u, udword& v, udword& w, udword i, udword nbu, udword nbu_nbv) { w = i / (nbu_nbv); Compute2DCoords(u, v, i - (w * nbu_nbv), nbu); } #endif // __ICEUTILS_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_AABBTree.cpp000066400000000000000000000537641207742442300232520ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for a versatile AABB tree. * \file OPC_AABBTree.cpp * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a generic AABB tree node. * * \class AABBTreeNode * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a generic AABB tree. * This is a vanilla AABB tree, without any particular optimization. It contains anonymous references to * user-provided primitives, which can theoretically be anything - triangles, boxes, etc. Each primitive * is surrounded by an AABB, regardless of the primitive's nature. When the primitive is a triangle, the * resulting tree can be converted into an optimized tree. If the primitive is a box, the resulting tree * can be used for culling - VFC or occlusion -, assuming you cull on a mesh-by-mesh basis (modern way). * * \class AABBTree * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBTreeNode::AABBTreeNode() : mPos (null), #ifndef OPC_NO_NEG_VANILLA_TREE mNeg (null), #endif mNodePrimitives (null), mNbPrimitives (0) { #ifdef OPC_USE_TREE_COHERENCE mBitmask = 0; #endif } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBTreeNode::~AABBTreeNode() { // Opcode 1.3: const AABBTreeNode* Pos = GetPos(); #ifndef OPC_NO_NEG_VANILLA_TREE const AABBTreeNode* Neg = GetNeg(); if(!(mPos&1)) DELETESINGLE(Pos); if(!(mNeg&1)) DELETESINGLE(Neg); #else if(!(mPos&1)) DELETEARRAY(Pos); #endif mNodePrimitives = null; // This was just a shortcut to the global list => no release mNbPrimitives = 0; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Splits the node along a given axis. * The list of indices is reorganized according to the split values. * \param axis [in] splitting axis index * \param builder [in] the tree builder * \return the number of primitives assigned to the first child * \warning this method reorganizes the internal list of primitives */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword AABBTreeNode::Split(udword axis, AABBTreeBuilder* builder) { // Get node split value float SplitValue = builder->GetSplittingValue(mNodePrimitives, mNbPrimitives, mBV, axis); udword NbPos = 0; // Loop through all node-related primitives. Their indices range from mNodePrimitives[0] to mNodePrimitives[mNbPrimitives-1]. // Those indices map the global list in the tree builder. for(udword i=0;iGetSplittingValue(Index, axis); // Reorganize the list of indices in this order: positive - negative. if(PrimitiveValue > SplitValue) { // Swap entries udword Tmp = mNodePrimitives[i]; mNodePrimitives[i] = mNodePrimitives[NbPos]; mNodePrimitives[NbPos] = Tmp; // Count primitives assigned to positive space NbPos++; } } return NbPos; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Subdivides the node. * * N * / \ * / \ * N/2 N/2 * / \ / \ * N/4 N/4 N/4 N/4 * (etc) * * A well-balanced tree should have a O(log n) depth. * A degenerate tree would have a O(n) depth. * Note a perfectly-balanced tree is not well-suited to collision detection anyway. * * \param builder [in] the tree builder * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeNode::Subdivide(AABBTreeBuilder* builder) { // Checkings if(!builder) return false; // Stop subdividing if we reach a leaf node. This is always performed here, // else we could end in trouble if user overrides this. if(mNbPrimitives==1) return true; // Let the user validate the subdivision if(!builder->ValidateSubdivision(mNodePrimitives, mNbPrimitives, mBV)) return true; bool ValidSplit = true; // Optimism... udword NbPos; if(builder->mSettings.mRules & SPLIT_LARGEST_AXIS) { // Find the largest axis to split along Point Extents; mBV.GetExtents(Extents); // Box extents udword Axis = Extents.LargestAxis(); // Index of largest axis // Split along the axis NbPos = Split(Axis, builder); // Check split validity if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false; } else if(builder->mSettings.mRules & SPLIT_SPLATTER_POINTS) { // Compute the means Point Means(0.0f, 0.0f, 0.0f); for(udword i=0;iGetSplittingValue(Index, 0); Means.y+=builder->GetSplittingValue(Index, 1); Means.z+=builder->GetSplittingValue(Index, 2); } Means/=float(mNbPrimitives); // Compute variances Point Vars(0.0f, 0.0f, 0.0f); for(udword i=0;iGetSplittingValue(Index, 0); volatile float Cy = builder->GetSplittingValue(Index, 1); volatile float Cz = builder->GetSplittingValue(Index, 2); Vars.x += (Cx - Means.x)*(Cx - Means.x); Vars.y += (Cy - Means.y)*(Cy - Means.y); Vars.z += (Cz - Means.z)*(Cz - Means.z); } Vars/=float(mNbPrimitives-1); // Choose axis with greatest variance udword Axis = Vars.LargestAxis(); // Split along the axis NbPos = Split(Axis, builder); // Check split validity if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false; } else if(builder->mSettings.mRules & SPLIT_BALANCED) { // Test 3 axis, take the best float Results[3]; NbPos = Split(0, builder); Results[0] = float(NbPos)/float(mNbPrimitives); NbPos = Split(1, builder); Results[1] = float(NbPos)/float(mNbPrimitives); NbPos = Split(2, builder); Results[2] = float(NbPos)/float(mNbPrimitives); Results[0]-=0.5f; Results[0]*=Results[0]; Results[1]-=0.5f; Results[1]*=Results[1]; Results[2]-=0.5f; Results[2]*=Results[2]; udword Min=0; if(Results[1]mSettings.mRules & SPLIT_BEST_AXIS) { // Test largest, then middle, then smallest axis... // Sort axis Point Extents; mBV.GetExtents(Extents); // Box extents udword SortedAxis[] = { 0, 1, 2 }; float* Keys = (float*)&Extents.x; for(udword j=0;j<3;j++) { for(udword i=0;i<2;i++) { if(Keys[SortedAxis[i]]mSettings.mRules & SPLIT_FIFTY) { // Don't even bother splitting (mainly a performance test) NbPos = mNbPrimitives>>1; } else return false; // Unknown splitting rules // Check the subdivision has been successful if(!ValidSplit) { // Here, all boxes lie in the same sub-space. Two strategies: // - if the tree *must* be complete, make an arbitrary 50-50 split // - else stop subdividing // if(builder->mSettings.mRules&SPLIT_COMPLETE) if(builder->mSettings.mLimit==1) { builder->IncreaseNbInvalidSplits(); NbPos = mNbPrimitives>>1; } else return true; } // Now create children and assign their pointers. if(builder->mNodeBase) { // We use a pre-allocated linear pool for complete trees [Opcode 1.3] AABBTreeNode* Pool = (AABBTreeNode*)builder->mNodeBase; udword Count = builder->GetCount() - 1; // Count begins to 1... // Set last bit to tell it shouldn't be freed ### pretty ugly, find a better way. Maybe one bit in mNbPrimitives ASSERT(!(udword(&Pool[Count+0])&1)); ASSERT(!(udword(&Pool[Count+1])&1)); mPos = EXWORD(&Pool[Count+0])|1; #ifndef OPC_NO_NEG_VANILLA_TREE mNeg = EXWORD(&Pool[Count+1])|1; #endif } else { // Non-complete trees and/or Opcode 1.2 allocate nodes on-the-fly #ifndef OPC_NO_NEG_VANILLA_TREE mPos = (EXWORD)new AABBTreeNode; CHECKALLOC(mPos); mNeg = (EXWORD)new AABBTreeNode; CHECKALLOC(mNeg); #else AABBTreeNode* PosNeg = new AABBTreeNode[2]; CHECKALLOC(PosNeg); mPos = (EXWORD)PosNeg; #endif } // Update stats builder->IncreaseCount(2); // Assign children AABBTreeNode* Pos = (AABBTreeNode*)GetPos(); AABBTreeNode* Neg = (AABBTreeNode*)GetNeg(); Pos->mNodePrimitives = &mNodePrimitives[0]; Pos->mNbPrimitives = NbPos; Neg->mNodePrimitives = &mNodePrimitives[NbPos]; Neg->mNbPrimitives = mNbPrimitives - NbPos; return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive hierarchy building in a top-down fashion. * \param builder [in] the tree builder */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeNode::_BuildHierarchy(AABBTreeBuilder* builder) { // 1) Compute the global box for current node. The box is stored in mBV. builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV); // 2) Subdivide current node Subdivide(builder); // 3) Recurse AABBTreeNode* Pos = (AABBTreeNode*)GetPos(); AABBTreeNode* Neg = (AABBTreeNode*)GetNeg(); if(Pos) Pos->_BuildHierarchy(builder); if(Neg) Neg->_BuildHierarchy(builder); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the tree (top-down). * \param builder [in] the tree builder */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeNode::_Refit(AABBTreeBuilder* builder) { // 1) Recompute the new global box for current node builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV); // 2) Recurse AABBTreeNode* Pos = (AABBTreeNode*)GetPos(); AABBTreeNode* Neg = (AABBTreeNode*)GetNeg(); if(Pos) Pos->_Refit(builder); if(Neg) Neg->_Refit(builder); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBTree::AABBTree() : mIndices(null), mPool(null), mTotalNbNodes(0) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBTree::~AABBTree() { Release(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Releases the tree. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTree::Release() { DELETEARRAY(mPool); DELETEARRAY(mIndices); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds a generic AABB tree from a tree builder. * \param builder [in] the tree builder * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTree::Build(AABBTreeBuilder* builder) { // Checkings if(!builder || !builder->mNbPrimitives) return false; // Release previous tree Release(); // Init stats builder->SetCount(1); builder->SetNbInvalidSplits(0); // Initialize indices. This list will be modified during build. mIndices = new udword[builder->mNbPrimitives]; CHECKALLOC(mIndices); // Identity permutation for(udword i=0;imNbPrimitives;i++) mIndices[i] = i; // Setup initial node. Here we have a complete permutation of the app's primitives. mNodePrimitives = mIndices; mNbPrimitives = builder->mNbPrimitives; // Use a linear array for complete trees (since we can predict the final number of nodes) [Opcode 1.3] // if(builder->mRules&SPLIT_COMPLETE) if(builder->mSettings.mLimit==1) { // Allocate a pool of nodes mPool = new AABBTreeNode[builder->mNbPrimitives*2 - 1]; builder->mNodeBase = mPool; // ### ugly ! } // Build the hierarchy _BuildHierarchy(builder); // Get back total number of nodes mTotalNbNodes = builder->GetCount(); // For complete trees, check the correct number of nodes has been created [Opcode 1.3] if(mPool) ASSERT(mTotalNbNodes==builder->mNbPrimitives*2 - 1); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the depth of the tree. * A well-balanced tree should have a log(n) depth. A degenerate tree O(n) depth. * \return depth of the tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword AABBTree::ComputeDepth() const { return Walk(null, null); // Use the walking code without callback } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Walks the tree, calling the user back for each node. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword AABBTree::Walk(WalkingCallback callback, void* user_data) const { // Call it without callback to compute max depth udword MaxDepth = 0; udword CurrentDepth = 0; struct Local { static void _Walk(const AABBTreeNode* current_node, udword& max_depth, udword& current_depth, WalkingCallback callback, void* user_data) { // Checkings if(!current_node) return; // Entering a new node => increase depth current_depth++; // Keep track of max depth if(current_depth>max_depth) max_depth = current_depth; // Callback if(callback && !(callback)(current_node, current_depth, user_data)) return; // Recurse if(current_node->GetPos()) { _Walk(current_node->GetPos(), max_depth, current_depth, callback, user_data); current_depth--; } if(current_node->GetNeg()) { _Walk(current_node->GetNeg(), max_depth, current_depth, callback, user_data); current_depth--; } } }; Local::_Walk(this, MaxDepth, CurrentDepth, callback, user_data); return MaxDepth; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the tree in a top-down way. * \param builder [in] the tree builder */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTree::Refit(AABBTreeBuilder* builder) { if(!builder) return false; _Refit(builder); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the tree in a bottom-up way. * \param builder [in] the tree builder */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTree::Refit2(AABBTreeBuilder* builder) { // Checkings if(!builder) return false; ASSERT(mPool); // Bottom-up update Point Min,Max; Point Min_,Max_; udword Index = mTotalNbNodes; while(Index--) { AABBTreeNode& Current = mPool[Index]; if(Current.IsLeaf()) { builder->ComputeGlobalBox(Current.GetPrimitives(), Current.GetNbPrimitives(), *(AABB*)Current.GetAABB()); } else { Current.GetPos()->GetAABB()->GetMin(Min); Current.GetPos()->GetAABB()->GetMax(Max); Current.GetNeg()->GetAABB()->GetMin(Min_); Current.GetNeg()->GetAABB()->GetMax(Max_); Min.Min(Min_); Max.Max(Max_); ((AABB*)Current.GetAABB())->SetMinMax(Min, Max); } } return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the number of bytes used by the tree. * \return number of bytes used */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword AABBTree::GetUsedBytes() const { udword TotalSize = mTotalNbNodes*GetNodeSize(); if(mIndices) TotalSize+=mNbPrimitives*sizeof(udword); return TotalSize; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the tree is a complete tree or not. * A complete tree is made of 2*N-1 nodes, where N is the number of primitives in the tree. * \return true for complete trees */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTree::IsComplete() const { return (GetNbNodes()==GetNbPrimitives()*2-1); } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_AABBTree.h000066400000000000000000000155601207742442300227070ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for a versatile AABB tree. * \file OPC_AABBTree.h * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_AABBTREE_H__ #define __OPC_AABBTREE_H__ #ifdef __x86_64 #define EXWORD uqword #else #define EXWORD udword #endif #ifdef OPC_NO_NEG_VANILLA_TREE //! TO BE DOCUMENTED #define IMPLEMENT_TREE(base_class, volume) \ public: \ /* Constructor / Destructor */ \ base_class(); \ ~base_class(); \ /* Data access */ \ inline_ const volume* Get##volume() const { return &mBV; } \ /* Clear the last bit */ \ inline_ const base_class* GetPos() const { return (const base_class*)(mPos&~1); } \ inline_ const base_class* GetNeg() const { const base_class* P = GetPos(); return P ? P+1 : null;} \ \ /* We don't need to test both nodes since we can't have one without the other */ \ inline_ bool IsLeaf() const { return !GetPos(); } \ \ /* Stats */ \ inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \ protected: \ /* Tree-independent data */ \ /* Following data always belong to the BV-tree, regardless of what the tree actually contains.*/ \ /* Whatever happens we need the two children and the enclosing volume.*/ \ volume mBV; /* Global bounding-volume enclosing all the node-related primitives */ \ EXWORD mPos; /* "Positive" & "Negative" children */ #else //! TO BE DOCUMENTED #define IMPLEMENT_TREE(base_class, volume) \ public: \ /* Constructor / Destructor */ \ base_class(); \ ~base_class(); \ /* Data access */ \ inline_ const volume* Get##volume() const { return &mBV; } \ /* Clear the last bit */ \ inline_ const base_class* GetPos() const { return (const base_class*)(mPos&~1); } \ inline_ const base_class* GetNeg() const { return (const base_class*)(mNeg&~1); } \ \ /* inline_ bool IsLeaf() const { return (!GetPos() && !GetNeg()); } */ \ /* We don't need to test both nodes since we can't have one without the other */ \ inline_ bool IsLeaf() const { return !GetPos(); } \ \ /* Stats */ \ inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \ protected: \ /* Tree-independent data */ \ /* Following data always belong to the BV-tree, regardless of what the tree actually contains.*/ \ /* Whatever happens we need the two children and the enclosing volume.*/ \ volume mBV; /* Global bounding-volume enclosing all the node-related primitives */ \ EXWORD mPos; /* "Positive" child */ \ EXOWRD mNeg; /* "Negative" child */ #endif typedef void (*CullingCallback) (udword nb_primitives, udword* node_primitives, BOOL need_clipping, void* user_data); class OPCODE_API AABBTreeNode { IMPLEMENT_TREE(AABBTreeNode, AABB) public: // Data access inline_ const udword* GetPrimitives() const { return mNodePrimitives; } inline_ udword GetNbPrimitives() const { return mNbPrimitives; } protected: // Tree-dependent data udword* mNodePrimitives; //!< Node-related primitives (shortcut to a position in mIndices below) udword mNbPrimitives; //!< Number of primitives for this node // Internal methods udword Split(udword axis, AABBTreeBuilder* builder); bool Subdivide(AABBTreeBuilder* builder); void _BuildHierarchy(AABBTreeBuilder* builder); void _Refit(AABBTreeBuilder* builder); }; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * User-callback, called for each node by the walking code. * \param current [in] current node * \param depth [in] current node's depth * \param user_data [in] user-defined data * \return true to recurse through children, else false to bypass them */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// typedef bool (*WalkingCallback) (const AABBTreeNode* current, udword depth, void* user_data); class OPCODE_API AABBTree : public AABBTreeNode { public: // Constructor / Destructor AABBTree(); ~AABBTree(); // Build bool Build(AABBTreeBuilder* builder); void Release(); // Data access inline_ const udword* GetIndices() const { return mIndices; } //!< Catch the indices inline_ udword GetNbNodes() const { return mTotalNbNodes; } //!< Catch the number of nodes // Infos bool IsComplete() const; // Stats udword ComputeDepth() const; udword GetUsedBytes() const; udword Walk(WalkingCallback callback, void* user_data) const; bool Refit(AABBTreeBuilder* builder); bool Refit2(AABBTreeBuilder* builder); private: udword* mIndices; //!< Indices in the app list. Indices are reorganized during build (permutation). AABBTreeNode* mPool; //!< Linear pool of nodes for complete trees. Null otherwise. [Opcode 1.3] // Stats udword mTotalNbNodes; //!< Number of nodes in the tree. }; #endif // __OPC_AABBTREE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_BaseModel.cpp000066400000000000000000000141311207742442300235610ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains base model interface. * \file OPC_BaseModel.cpp * \author Pierre Terdiman * \date May, 18, 2003 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * The base class for collision models. * * \class BaseModel * \author Pierre Terdiman * \version 1.3 * \date May, 18, 2003 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// OPCODECREATE::OPCODECREATE() { mIMesh = null; mSettings.mRules = SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER; mSettings.mLimit = 1; // Mandatory for complete trees mNoLeaf = true; mQuantized = true; #ifdef __MESHMERIZER_H__ mCollisionHull = false; #endif // __MESHMERIZER_H__ mKeepOriginal = false; mCanRemap = false; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// BaseModel::BaseModel() : mIMesh(null), mModelCode(0), mSource(null), mTree(null) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// BaseModel::~BaseModel() { ReleaseBase(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Releases everything. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void BaseModel::ReleaseBase() { DELETESINGLE(mSource); DELETESINGLE(mTree); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Creates an optimized tree according to user-settings, and setups mModelCode. * \param no_leaf [in] true for "no leaf" tree * \param quantized [in] true for quantized tree * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool BaseModel::CreateTree(bool no_leaf, bool quantized) { DELETESINGLE(mTree); // Setup model code if(no_leaf) mModelCode |= OPC_NO_LEAF; else mModelCode &= ~OPC_NO_LEAF; if(quantized) mModelCode |= OPC_QUANTIZED; else mModelCode &= ~OPC_QUANTIZED; // Create the correct class if(mModelCode & OPC_NO_LEAF) { if(mModelCode & OPC_QUANTIZED) mTree = new AABBQuantizedNoLeafTree; else mTree = new AABBNoLeafTree; } else { if(mModelCode & OPC_QUANTIZED) mTree = new AABBQuantizedTree; else mTree = new AABBCollisionTree; } CHECKALLOC(mTree); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the collision model. This can be used to handle dynamic meshes. Usage is: * 1. modify your mesh vertices (keep the topology constant!) * 2. refit the tree (call this method) * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool BaseModel::Refit() { // Refit the optimized tree return mTree->Refit(mIMesh); // Old code kept for reference : refit the source tree then rebuild ! // if(!mSource) return false; // // Ouch... // mSource->Refit(&mTB); // // Ouch... // return mTree->Build(mSource); } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_BaseModel.h000066400000000000000000000244541207742442300232370ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains base model interface. * \file OPC_BaseModel.h * \author Pierre Terdiman * \date May, 18, 2003 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_BASEMODEL_H__ #define __OPC_BASEMODEL_H__ //! Model creation structure struct OPCODE_API OPCODECREATE { //! Constructor OPCODECREATE(); MeshInterface* mIMesh; //!< Mesh interface (access to triangles & vertices) (*) BuildSettings mSettings; //!< Builder's settings bool mNoLeaf; //!< true => discard leaf nodes (else use a normal tree) bool mQuantized; //!< true => quantize the tree (else use a normal tree) #ifdef __MESHMERIZER_H__ bool mCollisionHull; //!< true => use convex hull + GJK #endif // __MESHMERIZER_H__ bool mKeepOriginal; //!< true => keep a copy of the original tree (debug purpose) bool mCanRemap; //!< true => allows OPCODE to reorganize client arrays // (*) This pointer is saved internally and used by OPCODE until collision structures are released, // so beware of the object's lifetime. }; enum ModelFlag { OPC_QUANTIZED = (1<<0), //!< Compressed/uncompressed tree OPC_NO_LEAF = (1<<1), //!< Leaf/NoLeaf tree OPC_SINGLE_NODE = (1<<2) //!< Special case for 1-node models }; class OPCODE_API BaseModel { public: // Constructor/Destructor BaseModel(); virtual ~BaseModel(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds a collision model. * \param create [in] model creation structure * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// virtual bool Build(const OPCODECREATE& create) = 0; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of bytes used by the tree. * \return amount of bytes used */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// virtual udword GetUsedBytes() const = 0; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the collision model. This can be used to handle dynamic meshes. Usage is: * 1. modify your mesh vertices (keep the topology constant!) * 2. refit the tree (call this method) * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// virtual bool Refit(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the source tree. * \return generic tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ const AABBTree* GetSourceTree() const { return mSource; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the tree. * \return the collision tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ const AABBOptimizedTree* GetTree() const { return mTree; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the tree. * \return the collision tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ AABBOptimizedTree* GetTree() { return mTree; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of nodes in the tree. * Should be 2*N-1 for normal trees and N-1 for optimized ones. * \return number of nodes */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbNodes() const { return mTree->GetNbNodes(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks whether the tree has leaf nodes or not. * \return true if the tree has leaf nodes (normal tree), else false (optimized tree) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL HasLeafNodes() const { return !(mModelCode & OPC_NO_LEAF); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks whether the tree is quantized or not. * \return true if the tree is quantized */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL IsQuantized() const { return mModelCode & OPC_QUANTIZED; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks whether the model has a single node or not. This special case must be handled separately. * \return true if the model has only 1 node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL HasSingleNode() const { return mModelCode & OPC_SINGLE_NODE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the model's code. * \return model's code */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetModelCode() const { return mModelCode; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the mesh interface. * \return mesh interface */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ const MeshInterface* GetMeshInterface() const { return mIMesh; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Sets the mesh interface. * \param imesh [in] mesh interface */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetMeshInterface(const MeshInterface* imesh) { mIMesh = imesh; } protected: const MeshInterface* mIMesh; //!< User-defined mesh interface udword mModelCode; //!< Model code = combination of ModelFlag(s) AABBTree* mSource; //!< Original source tree AABBOptimizedTree* mTree; //!< Optimized tree owned by the model // Internal methods void ReleaseBase(); bool CreateTree(bool no_leaf, bool quantized); }; #endif //__OPC_BASEMODEL_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_BoxBoxOverlap.h000066400000000000000000000172031207742442300241300ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * OBB-OBB overlap test using the separating axis theorem. * - original code by Gomez / Gamasutra (similar to Gottschalk's one in RAPID) * - optimized for AABB trees by computing the rotation matrix once (SOLID-fashion) * - the fabs matrix is precomputed as well and epsilon-tweaked (RAPID-style, we found this almost mandatory) * - Class III axes can be disabled... (SOLID & Intel fashion) * - ...or enabled to perform some profiling * - CPU comparisons used when appropriate * - lazy evaluation sometimes saves some work in case of early exits (unlike SOLID) * * \param ea [in] extents from box A * \param ca [in] center from box A * \param eb [in] extents from box B * \param cb [in] center from box B * \return true if boxes overlap */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL AABBTreeCollider::BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb) { // Stats mNbBVBVTests++; float t,t2; // Class I : A's basis vectors float Tx = (mR1to0.m[0][0]*cb.x + mR1to0.m[1][0]*cb.y + mR1to0.m[2][0]*cb.z) + mT1to0.x - ca.x; t = ea.x + eb.x*mAR.m[0][0] + eb.y*mAR.m[1][0] + eb.z*mAR.m[2][0]; if(GREATER(Tx, t)) return FALSE; float Ty = (mR1to0.m[0][1]*cb.x + mR1to0.m[1][1]*cb.y + mR1to0.m[2][1]*cb.z) + mT1to0.y - ca.y; t = ea.y + eb.x*mAR.m[0][1] + eb.y*mAR.m[1][1] + eb.z*mAR.m[2][1]; if(GREATER(Ty, t)) return FALSE; float Tz = (mR1to0.m[0][2]*cb.x + mR1to0.m[1][2]*cb.y + mR1to0.m[2][2]*cb.z) + mT1to0.z - ca.z; t = ea.z + eb.x*mAR.m[0][2] + eb.y*mAR.m[1][2] + eb.z*mAR.m[2][2]; if(GREATER(Tz, t)) return FALSE; // Class II : B's basis vectors t = Tx*mR1to0.m[0][0] + Ty*mR1to0.m[0][1] + Tz*mR1to0.m[0][2]; t2 = ea.x*mAR.m[0][0] + ea.y*mAR.m[0][1] + ea.z*mAR.m[0][2] + eb.x; if(GREATER(t, t2)) return FALSE; t = Tx*mR1to0.m[1][0] + Ty*mR1to0.m[1][1] + Tz*mR1to0.m[1][2]; t2 = ea.x*mAR.m[1][0] + ea.y*mAR.m[1][1] + ea.z*mAR.m[1][2] + eb.y; if(GREATER(t, t2)) return FALSE; t = Tx*mR1to0.m[2][0] + Ty*mR1to0.m[2][1] + Tz*mR1to0.m[2][2]; t2 = ea.x*mAR.m[2][0] + ea.y*mAR.m[2][1] + ea.z*mAR.m[2][2] + eb.z; if(GREATER(t, t2)) return FALSE; // Class III : 9 cross products // Cool trick: always perform the full test for first level, regardless of settings. // That way pathological cases (such as the pencils scene) are quickly rejected anyway ! if(mFullBoxBoxTest || mNbBVBVTests==1) { t = Tz*mR1to0.m[0][1] - Ty*mR1to0.m[0][2]; t2 = ea.y*mAR.m[0][2] + ea.z*mAR.m[0][1] + eb.y*mAR.m[2][0] + eb.z*mAR.m[1][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B0 t = Tz*mR1to0.m[1][1] - Ty*mR1to0.m[1][2]; t2 = ea.y*mAR.m[1][2] + ea.z*mAR.m[1][1] + eb.x*mAR.m[2][0] + eb.z*mAR.m[0][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B1 t = Tz*mR1to0.m[2][1] - Ty*mR1to0.m[2][2]; t2 = ea.y*mAR.m[2][2] + ea.z*mAR.m[2][1] + eb.x*mAR.m[1][0] + eb.y*mAR.m[0][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B2 t = Tx*mR1to0.m[0][2] - Tz*mR1to0.m[0][0]; t2 = ea.x*mAR.m[0][2] + ea.z*mAR.m[0][0] + eb.y*mAR.m[2][1] + eb.z*mAR.m[1][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B0 t = Tx*mR1to0.m[1][2] - Tz*mR1to0.m[1][0]; t2 = ea.x*mAR.m[1][2] + ea.z*mAR.m[1][0] + eb.x*mAR.m[2][1] + eb.z*mAR.m[0][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B1 t = Tx*mR1to0.m[2][2] - Tz*mR1to0.m[2][0]; t2 = ea.x*mAR.m[2][2] + ea.z*mAR.m[2][0] + eb.x*mAR.m[1][1] + eb.y*mAR.m[0][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B2 t = Ty*mR1to0.m[0][0] - Tx*mR1to0.m[0][1]; t2 = ea.x*mAR.m[0][1] + ea.y*mAR.m[0][0] + eb.y*mAR.m[2][2] + eb.z*mAR.m[1][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B0 t = Ty*mR1to0.m[1][0] - Tx*mR1to0.m[1][1]; t2 = ea.x*mAR.m[1][1] + ea.y*mAR.m[1][0] + eb.x*mAR.m[2][2] + eb.z*mAR.m[0][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B1 t = Ty*mR1to0.m[2][0] - Tx*mR1to0.m[2][1]; t2 = ea.x*mAR.m[2][1] + ea.y*mAR.m[2][0] + eb.x*mAR.m[1][2] + eb.y*mAR.m[0][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B2 } return TRUE; } // Modified! // Only AABBTreeCollider #if 0 //! A dedicated version when one box is constant inline_ BOOL OBBCollider::BoxBoxOverlap(const Point& extents, const Point& center) { // Stats mNbVolumeBVTests++; float t,t2; // Class I : A's basis vectors float Tx = mTBoxToModel.x - center.x; t = extents.x + mBBx1; if(GREATER(Tx, t)) return FALSE; float Ty = mTBoxToModel.y - center.y; t = extents.y + mBBy1; if(GREATER(Ty, t)) return FALSE; float Tz = mTBoxToModel.z - center.z; t = extents.z + mBBz1; if(GREATER(Tz, t)) return FALSE; // Class II : B's basis vectors t = Tx*mRBoxToModel.m[0][0] + Ty*mRBoxToModel.m[0][1] + Tz*mRBoxToModel.m[0][2]; t2 = extents.x*mAR.m[0][0] + extents.y*mAR.m[0][1] + extents.z*mAR.m[0][2] + mBoxExtents.x; if(GREATER(t, t2)) return FALSE; t = Tx*mRBoxToModel.m[1][0] + Ty*mRBoxToModel.m[1][1] + Tz*mRBoxToModel.m[1][2]; t2 = extents.x*mAR.m[1][0] + extents.y*mAR.m[1][1] + extents.z*mAR.m[1][2] + mBoxExtents.y; if(GREATER(t, t2)) return FALSE; t = Tx*mRBoxToModel.m[2][0] + Ty*mRBoxToModel.m[2][1] + Tz*mRBoxToModel.m[2][2]; t2 = extents.x*mAR.m[2][0] + extents.y*mAR.m[2][1] + extents.z*mAR.m[2][2] + mBoxExtents.z; if(GREATER(t, t2)) return FALSE; // Class III : 9 cross products // Cool trick: always perform the full test for first level, regardless of settings. // That way pathological cases (such as the pencils scene) are quickly rejected anyway ! if(mFullBoxBoxTest || mNbVolumeBVTests==1) { t = Tz*mRBoxToModel.m[0][1] - Ty*mRBoxToModel.m[0][2]; t2 = extents.y*mAR.m[0][2] + extents.z*mAR.m[0][1] + mBB_1; if(GREATER(t, t2)) return FALSE; // L = A0 x B0 t = Tz*mRBoxToModel.m[1][1] - Ty*mRBoxToModel.m[1][2]; t2 = extents.y*mAR.m[1][2] + extents.z*mAR.m[1][1] + mBB_2; if(GREATER(t, t2)) return FALSE; // L = A0 x B1 t = Tz*mRBoxToModel.m[2][1] - Ty*mRBoxToModel.m[2][2]; t2 = extents.y*mAR.m[2][2] + extents.z*mAR.m[2][1] + mBB_3; if(GREATER(t, t2)) return FALSE; // L = A0 x B2 t = Tx*mRBoxToModel.m[0][2] - Tz*mRBoxToModel.m[0][0]; t2 = extents.x*mAR.m[0][2] + extents.z*mAR.m[0][0] + mBB_4; if(GREATER(t, t2)) return FALSE; // L = A1 x B0 t = Tx*mRBoxToModel.m[1][2] - Tz*mRBoxToModel.m[1][0]; t2 = extents.x*mAR.m[1][2] + extents.z*mAR.m[1][0] + mBB_5; if(GREATER(t, t2)) return FALSE; // L = A1 x B1 t = Tx*mRBoxToModel.m[2][2] - Tz*mRBoxToModel.m[2][0]; t2 = extents.x*mAR.m[2][2] + extents.z*mAR.m[2][0] + mBB_6; if(GREATER(t, t2)) return FALSE; // L = A1 x B2 t = Ty*mRBoxToModel.m[0][0] - Tx*mRBoxToModel.m[0][1]; t2 = extents.x*mAR.m[0][1] + extents.y*mAR.m[0][0] + mBB_7; if(GREATER(t, t2)) return FALSE; // L = A2 x B0 t = Ty*mRBoxToModel.m[1][0] - Tx*mRBoxToModel.m[1][1]; t2 = extents.x*mAR.m[1][1] + extents.y*mAR.m[1][0] + mBB_8; if(GREATER(t, t2)) return FALSE; // L = A2 x B1 t = Ty*mRBoxToModel.m[2][0] - Tx*mRBoxToModel.m[2][1]; t2 = extents.x*mAR.m[2][1] + extents.y*mAR.m[2][0] + mBB_9; if(GREATER(t, t2)) return FALSE; // L = A2 x B2 } return TRUE; } //! A special version for 2 axis-aligned boxes inline_ BOOL AABBCollider::AABBAABBOverlap(const Point& extents, const Point& center) { // Stats mNbVolumeBVTests++; float tx = mBox.mCenter.x - center.x; float ex = extents.x + mBox.mExtents.x; if(GREATER(tx, ex)) return FALSE; float ty = mBox.mCenter.y - center.y; float ey = extents.y + mBox.mExtents.y; if(GREATER(ty, ey)) return FALSE; float tz = mBox.mCenter.z - center.z; float ez = extents.z + mBox.mExtents.z; if(GREATER(tz, ez)) return FALSE; return TRUE; } #endif choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_BoxPruning.cpp000066400000000000000000000322221207742442300240220ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for box pruning. * \file IceBoxPruning.cpp * \author Pierre Terdiman * \date January, 29, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// You could use a complex sweep-and-prune as implemented in I-Collide. You could use a complex hashing scheme as implemented in V-Clip or recently in ODE it seems. You could use a "Recursive Dimensional Clustering" algorithm as implemented in GPG2. Or you could use this. Faster ? I don't know. Probably not. It would be a shame. But who knows ? Easier ? Definitely. Enjoy the sheer simplicity. /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; inline_ void FindRunningIndex(udword& index, float* array, udword* sorted, int last, float max) { int First=index; while(First<=last) { index = (First+last)>>1; if(max>array[sorted[index]]) First = index+1; else last = index-1; } } // ### could be log(n) ! // and maybe use cmp integers // InsertionSort has better coherence, RadixSort is better for one-shot queries. #define PRUNING_SORTER RadixSort //#define PRUNING_SORTER InsertionSort // Static for coherence static PRUNING_SORTER* gCompletePruningSorter = null; static PRUNING_SORTER* gBipartitePruningSorter0 = null; static PRUNING_SORTER* gBipartitePruningSorter1 = null; inline_ PRUNING_SORTER* GetCompletePruningSorter() { if(!gCompletePruningSorter) gCompletePruningSorter = new PRUNING_SORTER; return gCompletePruningSorter; } inline_ PRUNING_SORTER* GetBipartitePruningSorter0() { if(!gBipartitePruningSorter0) gBipartitePruningSorter0 = new PRUNING_SORTER; return gBipartitePruningSorter0; } inline_ PRUNING_SORTER* GetBipartitePruningSorter1() { if(!gBipartitePruningSorter1) gBipartitePruningSorter1 = new PRUNING_SORTER; return gBipartitePruningSorter1; } void ReleasePruningSorters() { DELETESINGLE(gBipartitePruningSorter1); DELETESINGLE(gBipartitePruningSorter0); DELETESINGLE(gCompletePruningSorter); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set. * \param nb0 [in] number of boxes in the first set * \param array0 [in] array of boxes for the first set * \param nb1 [in] number of boxes in the second set * \param array1 [in] array of boxes for the second set * \param pairs [out] array of overlapping pairs * \param axes [in] projection order (0,2,1 is often best) * \return true if success. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Opcode::BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes) { // Checkings if(!nb0 || !array0 || !nb1 || !array1) return false; // Catch axes udword Axis0 = axes.mAxis0; udword Axis1 = axes.mAxis1; udword Axis2 = axes.mAxis2; // Allocate some temporary data float* MinPosList0 = new float[nb0]; float* MinPosList1 = new float[nb1]; // 1) Build main lists using the primary axis for(udword i=0;iGetMin(Axis0); for(udword i=0;iGetMin(Axis0); // 2) Sort the lists PRUNING_SORTER* RS0 = GetBipartitePruningSorter0(); PRUNING_SORTER* RS1 = GetBipartitePruningSorter1(); const udword* Sorted0 = RS0->Sort(MinPosList0, nb0).GetRanks(); const udword* Sorted1 = RS1->Sort(MinPosList1, nb1).GetRanks(); // 3) Prune the lists udword Index0, Index1; const udword* const LastSorted0 = &Sorted0[nb0]; const udword* const LastSorted1 = &Sorted1[nb1]; const udword* RunningAddress0 = Sorted0; const udword* RunningAddress1 = Sorted1; while(RunningAddress1GetMax(Axis0)) { if(array0[Index0]->Intersect(*array1[Index1], Axis1)) { if(array0[Index0]->Intersect(*array1[Index1], Axis2)) { pairs.AddPair(Index0, Index1); } } } } //// while(RunningAddress0GetMax(Axis0)) { if(array0[Index1]->Intersect(*array1[Index0], Axis1)) { if(array0[Index1]->Intersect(*array1[Index0], Axis2)) { pairs.AddPair(Index1, Index0); } } } } DELETEARRAY(MinPosList1); DELETEARRAY(MinPosList0); return true; } #define ORIGINAL_VERSION //#define JOAKIM /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set. * \param nb [in] number of boxes * \param array [in] array of boxes * \param pairs [out] array of overlapping pairs * \param axes [in] projection order (0,2,1 is often best) * \return true if success. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Opcode::CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes) { // Checkings if(!nb || !array) return false; // Catch axes udword Axis0 = axes.mAxis0; udword Axis1 = axes.mAxis1; udword Axis2 = axes.mAxis2; #ifdef ORIGINAL_VERSION // Allocate some temporary data // float* PosList = new float[nb]; float* PosList = new float[nb+1]; // 1) Build main list using the primary axis for(udword i=0;iGetMin(Axis0); PosList[nb++] = MAX_FLOAT; // 2) Sort the list PRUNING_SORTER* RS = GetCompletePruningSorter(); const udword* Sorted = RS->Sort(PosList, nb).GetRanks(); // 3) Prune the list const udword* const LastSorted = &Sorted[nb]; const udword* RunningAddress = Sorted; udword Index0, Index1; while(RunningAddressGetMax(Axis0)) while(PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0)) { // if(Index0!=Index1) // { if(array[Index0]->Intersect(*array[Index1], Axis1)) { if(array[Index0]->Intersect(*array[Index1], Axis2)) { pairs.AddPair(Index0, Index1); } } // } } } } DELETEARRAY(PosList); #endif #ifdef JOAKIM // Allocate some temporary data // float* PosList = new float[nb]; float* MinList = new float[nb+1]; // 1) Build main list using the primary axis for(udword i=0;iGetMin(Axis0); MinList[nb] = MAX_FLOAT; // 2) Sort the list PRUNING_SORTER* RS = GetCompletePruningSorter(); udword* Sorted = RS->Sort(MinList, nb+1).GetRanks(); // 3) Prune the list // const udword* const LastSorted = &Sorted[nb]; // const udword* const LastSorted = &Sorted[nb-1]; const udword* RunningAddress = Sorted; udword Index0, Index1; // while(RunningAddressGetMax(Axis0)) // float CurrentMin = array[Index0]->GetMin(Axis0); float CurrentMax = array[Index0]->GetMax(Axis0); while(MinList[Index1 = *RunningAddress2] <= CurrentMax) // while(PosList[Index1 = *RunningAddress] <= CurrentMax) { // if(Index0!=Index1) // { if(array[Index0]->Intersect(*array[Index1], Axis1)) { if(array[Index0]->Intersect(*array[Index1], Axis2)) { pairs.AddPair(Index0, Index1); } } // } RunningAddress2++; // RunningAddress++; } } } DELETEARRAY(MinList); #endif return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Brute-force versions are kept: // - to check the optimized versions return the correct list of intersections // - to check the speed of the optimized code against the brute-force one /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Brute-force bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set. * \param nb0 [in] number of boxes in the first set * \param array0 [in] array of boxes for the first set * \param nb1 [in] number of boxes in the second set * \param array1 [in] array of boxes for the second set * \param pairs [out] array of overlapping pairs * \return true if success. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Opcode::BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs) { // Checkings if(!nb0 || !array0 || !nb1 || !array1) return false; // Brute-force nb0*nb1 overlap tests for(udword i=0;iIntersect(*array1[j])) pairs.AddPair(i, j); } } return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set. * \param nb [in] number of boxes * \param array [in] array of boxes * \param pairs [out] array of overlapping pairs * \return true if success. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Opcode::BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs) { // Checkings if(!nb || !array) return false; // Brute-force n(n-1)/2 overlap tests for(udword i=0;iIntersect(*array[j])) pairs.AddPair(i, j); } } return true; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_BoxPruning.h000066400000000000000000000035621207742442300234740ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for box pruning. * \file IceBoxPruning.h * \author Pierre Terdiman * \date January, 29, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_BOXPRUNING_H__ #define __OPC_BOXPRUNING_H__ // Optimized versions FUNCTION OPCODE_API bool CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes); FUNCTION OPCODE_API bool BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes); // Brute-force versions FUNCTION OPCODE_API bool BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs); FUNCTION OPCODE_API bool BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs); #endif //__OPC_BOXPRUNING_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Collider.cpp000066400000000000000000000054151207742442300234700ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains base collider class. * \file OPC_Collider.cpp * \author Pierre Terdiman * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains the abstract class for colliders. * * \class Collider * \author Pierre Terdiman * \version 1.3 * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Collider::Collider() : mFlags (0), mCurrentModel (null), mIMesh (null) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Collider::~Collider() { } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Collider.h000066400000000000000000000242351207742442300231360ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains base collider class. * \file OPC_Collider.h * \author Pierre Terdiman * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_COLLIDER_H__ #define __OPC_COLLIDER_H__ enum CollisionFlag { OPC_FIRST_CONTACT = (1<<0), //!< Report all contacts (false) or only first one (true) OPC_TEMPORAL_COHERENCE = (1<<1), //!< Use temporal coherence or not OPC_CONTACT = (1<<2), //!< Final contact status after a collision query OPC_TEMPORAL_HIT = (1<<3), //!< There has been an early exit due to temporal coherence OPC_NO_PRIMITIVE_TESTS = (1<<4), //!< Keep or discard primitive-bv tests in leaf nodes (volume-mesh queries) OPC_CONTACT_FOUND = OPC_FIRST_CONTACT | OPC_CONTACT, OPC_TEMPORAL_CONTACT = OPC_TEMPORAL_HIT | OPC_CONTACT, OPC_FORCE_DWORD = 0x7fffffff }; class OPCODE_API Collider { public: // Constructor / Destructor Collider(); virtual ~Collider(); // Collision report /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the last collision status after a collision query. * \return true if a collision occured */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL GetContactStatus() const { return mFlags & OPC_CONTACT; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the "first contact" mode. * \return true if "first contact" mode is on */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL FirstContactEnabled() const { return mFlags & OPC_FIRST_CONTACT; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the temporal coherence mode. * \return true if temporal coherence is on */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL TemporalCoherenceEnabled() const { return mFlags & OPC_TEMPORAL_COHERENCE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks a first contact has already been found. * \return true if a first contact has been found and we can stop a query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL ContactFound() const { return (mFlags&OPC_CONTACT_FOUND)==OPC_CONTACT_FOUND; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks there's been an early exit due to temporal coherence; * \return true if a temporal hit has occured */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL TemporalHit() const { return mFlags & OPC_TEMPORAL_HIT; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks primitive tests are enabled; * \return true if primitive tests must be skipped */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL SkipPrimitiveTests() const { return mFlags & OPC_NO_PRIMITIVE_TESTS; } // Settings /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Reports all contacts (false) or first contact only (true) * \param flag [in] true for first contact, false for all contacts * \see SetTemporalCoherence(bool flag) * \see ValidateSettings() */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetFirstContact(bool flag) { if(flag) mFlags |= OPC_FIRST_CONTACT; else mFlags &= ~OPC_FIRST_CONTACT; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Enable/disable temporal coherence. * \param flag [in] true to enable temporal coherence, false to discard it * \see SetFirstContact(bool flag) * \see ValidateSettings() */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetTemporalCoherence(bool flag) { if(flag) mFlags |= OPC_TEMPORAL_COHERENCE; else mFlags &= ~OPC_TEMPORAL_COHERENCE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Enable/disable primitive tests. * \param flag [in] true to enable primitive tests, false to discard them */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetPrimitiveTests(bool flag) { if(!flag) mFlags |= OPC_NO_PRIMITIVE_TESTS; else mFlags &= ~OPC_NO_PRIMITIVE_TESTS; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider. * \return null if everything is ok, else a string describing the problem */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// virtual const char* ValidateSettings() = 0; protected: udword mFlags; //!< Bit flags const BaseModel* mCurrentModel; //!< Current model for collision query (owner of touched faces) // User mesh interface const MeshInterface* mIMesh; //!< User-defined mesh interface // Internal methods /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups current collision model * \param model [in] current collision model * \return TRUE if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL Setup(const BaseModel* model) { // Keep track of current model mCurrentModel = model; if(!mCurrentModel) return FALSE; mIMesh = model->GetMeshInterface(); return mIMesh!=null; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Initializes a query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// virtual inline_ void InitQuery() { mFlags &= ~OPC_TEMPORAL_CONTACT; } }; #endif // __OPC_COLLIDER_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Common.cpp000066400000000000000000000052521207742442300231620ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains common classes & defs used in OPCODE. * \file OPC_Common.cpp * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * An AABB dedicated to collision detection. * We don't use the generic AABB class included in ICE, since it can be a Min/Max or a Center/Extents one (depends * on compilation flags). Since the Center/Extents model is more efficient in collision detection, it was worth * using an extra special class. * * \class CollisionAABB * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A quantized AABB. * Center/Extent model, using 16-bits integers. * * \class QuantizedAABB * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Common.h000066400000000000000000000137341207742442300226330ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains common classes & defs used in OPCODE. * \file OPC_Common.h * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_COMMON_H__ #define __OPC_COMMON_H__ // [GOTTFRIED]: Just a small change for readability. #ifdef OPC_CPU_COMPARE #define GREATER(x, y) AIR(x) > IR(y) #else #define GREATER(x, y) fabsf(x) > (y) #endif class OPCODE_API CollisionAABB { public: //! Constructor inline_ CollisionAABB() {} //! Constructor inline_ CollisionAABB(const AABB& b) { b.GetCenter(mCenter); b.GetExtents(mExtents); } //! Destructor inline_ ~CollisionAABB() {} //! Get min point of the box inline_ void GetMin(Point& min) const { min = mCenter - mExtents; } //! Get max point of the box inline_ void GetMax(Point& max) const { max = mCenter + mExtents; } //! Get component of the box's min point along a given axis inline_ float GetMin(udword axis) const { return mCenter[axis] - mExtents[axis]; } //! Get component of the box's max point along a given axis inline_ float GetMax(udword axis) const { return mCenter[axis] + mExtents[axis]; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Setups an AABB from min & max vectors. * \param min [in] the min point * \param max [in] the max point */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetMinMax(const Point& min, const Point& max) { mCenter = (max + min)*0.5f; mExtents = (max - min)*0.5f; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks a box is inside another box. * \param box [in] the other box * \return true if current box is inside input box */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL IsInside(const CollisionAABB& box) const { if(box.GetMin(0)>GetMin(0)) return FALSE; if(box.GetMin(1)>GetMin(1)) return FALSE; if(box.GetMin(2)>GetMin(2)) return FALSE; if(box.GetMax(0) 2){ mType = SSV_LSS; mPoint0 = mCenter; mPoint0[maxAxis] -= mExtents[maxAxis]; mPoint1 = mCenter; mPoint1[maxAxis] += mExtents[maxAxis]; Point p = mExtents; p[maxAxis] = 0; mRadius = p.Magnitude(); }else{ mType = SSV_PSS; mRadius = mExtents.Magnitude(); } } #endif }; class OPCODE_API QuantizedAABB { public: //! Constructor inline_ QuantizedAABB() {} //! Destructor inline_ ~QuantizedAABB() {} sword mCenter[3]; //!< Quantized center uword mExtents[3]; //!< Quantized extents }; //! Quickly rotates & translates a vector inline_ void TransformPoint(Point& dest, const Point& source, const Matrix3x3& rot, const Point& trans) { dest.x = trans.x + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0]; dest.y = trans.y + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1]; dest.z = trans.z + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2]; } #endif //__OPC_COMMON_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_HybridModel.cpp000066400000000000000000000400731207742442300241340ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for hybrid models. * \file OPC_HybridModel.cpp * \author Pierre Terdiman * \date May, 18, 2003 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * An hybrid collision model. * * The problem : * * Opcode really shines for mesh-mesh collision, especially when meshes are deeply overlapping * (it typically outperforms RAPID in those cases). * * Unfortunately this is not the typical scenario in games. * * For close-proximity cases, especially for volume-mesh queries, it's relatively easy to run faster * than Opcode, that suffers from a relatively high setup time. * * In particular, Opcode's "vanilla" trees in those cases -can- run faster. They can also use -less- * memory than the optimized ones, when you let the system stop at ~10 triangles / leaf for example * (i.e. when you don't use "complete" trees). However, those trees tend to fragment memory quite a * lot, increasing cache misses : since they're not "complete", we can't predict the final number of * nodes and we have to allocate nodes on-the-fly. For the same reasons we can't use Opcode's "optimized" * trees here, since they rely on a known layout to perform the "optimization". * * Hybrid trees : * * Hybrid trees try to combine best of both worlds : * * - they use a maximum limit of 16 triangles/leaf. "16" is used so that we'll be able to save the * number of triangles using 4 bits only. * * - they're still "complete" trees thanks to a two-passes building phase. First we create a "vanilla" * AABB-tree with Opcode, limited to 16 triangles/leaf. Then we create a *second* vanilla tree, this * time using the leaves of the first one. The trick is : this second tree is now "complete"... so we * can further transform it into an Opcode's optimized tree. * * - then we run the collision queries on that standard Opcode tree. The only difference is that leaf * nodes contain indices to leaf nodes of another tree. Also, we have to skip all primitive tests in * Opcode optimized trees, since our leaves don't contain triangles anymore. * * - finally, for each collided leaf, we simply loop through 16 triangles max, and collide them with * the bounding volume used in the query (we only support volume-vs-mesh queries here, not mesh-vs-mesh) * * All of that is wrapped in this "hybrid model" that contains the minimal data required for this to work. * It's a mix between old "vanilla" trees, and old "optimized" trees. * * Extra advantages: * * - If we use them for dynamic models, we're left with a very small number of leaf nodes to refit. It * might be a bit faster since we have less nodes to write back. * * - In rigid body simulation, using temporal coherence and sleeping objects greatly reduce the actual * influence of one tree over another (i.e. the speed difference is often invisible). So memory is really * the key element to consider, and in this regard hybrid trees are just better. * * Information to take home: * - they use less ram * - they're not slower (they're faster or slower depending on cases, overall there's no significant * difference *as long as objects don't interpenetrate too much* - in which case Opcode's optimized trees * are still notably faster) * * \class HybridModel * \author Pierre Terdiman * \version 1.3 * \date May, 18, 2003 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// HybridModel::HybridModel() : mNbLeaves (0), mNbPrimitives (0), mTriangles (null), mIndices (null) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// HybridModel::~HybridModel() { Release(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Releases everything. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void HybridModel::Release() { ReleaseBase(); DELETEARRAY(mIndices); DELETEARRAY(mTriangles); mNbLeaves = 0; mNbPrimitives = 0; } struct Internal { Internal() { mNbLeaves = 0; mLeaves = null; mTriangles = null; mBase = null; } ~Internal() { DELETEARRAY(mLeaves); } udword mNbLeaves; AABB* mLeaves; LeafTriangles* mTriangles; const udword* mBase; }; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds a collision model. * \param create [in] model creation structure * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool HybridModel::Build(const OPCODECREATE& create) { // 1) Checkings if(!create.mIMesh || !create.mIMesh->IsValid()) return false; // Look for degenerate faces. udword NbDegenerate = create.mIMesh->CheckTopology(); if(NbDegenerate) Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate); // We continue nonetheless.... Release(); // Make sure previous tree has been discarded // 1-1) Setup mesh interface automatically SetMeshInterface(create.mIMesh); bool Status = false; AABBTree* LeafTree = null; Internal Data; // 2) Build a generic AABB Tree. mSource = new AABBTree; CHECKALLOC(mSource); // 2-1) Setup a builder. Our primitives here are triangles from input mesh, // so we use an AABBTreeOfTrianglesBuilder..... { AABBTreeOfTrianglesBuilder TB; TB.mIMesh = create.mIMesh; TB.mNbPrimitives = create.mIMesh->GetNbTriangles(); TB.mSettings = create.mSettings; TB.mSettings.mLimit = 16; // ### Hardcoded, but maybe we could let the user choose 8 / 16 / 32 ... if(!mSource->Build(&TB)) goto FreeAndExit; } // 2-2) Here's the trick : create *another* AABB tree using the leaves of the first one (which are boxes, this time) struct Local { // A callback to count leaf nodes static bool CountLeaves(const AABBTreeNode* current, udword depth, void* user_data) { if(current->IsLeaf()) { Internal* Data = (Internal*)user_data; Data->mNbLeaves++; } return true; } // A callback to setup leaf nodes in our internal structures static bool SetupLeafData(const AABBTreeNode* current, udword depth, void* user_data) { if(current->IsLeaf()) { Internal* Data = (Internal*)user_data; // Get current leaf's box Data->mLeaves[Data->mNbLeaves] = *current->GetAABB(); // Setup leaf data udword Index = (udword(current->GetPrimitives()) - udword(Data->mBase))/sizeof(udword); Data->mTriangles[Data->mNbLeaves].SetData(current->GetNbPrimitives(), Index); Data->mNbLeaves++; } return true; } }; // Walk the tree & count number of leaves Data.mNbLeaves = 0; mSource->Walk(Local::CountLeaves, &Data); mNbLeaves = Data.mNbLeaves; // Keep track of it // Special case for 1-leaf meshes if(mNbLeaves==1) { mModelCode |= OPC_SINGLE_NODE; Status = true; goto FreeAndExit; } // Allocate our structures Data.mLeaves = new AABB[Data.mNbLeaves]; CHECKALLOC(Data.mLeaves); mTriangles = new LeafTriangles[Data.mNbLeaves]; CHECKALLOC(mTriangles); // Walk the tree again & setup leaf data Data.mTriangles = mTriangles; Data.mBase = mSource->GetIndices(); Data.mNbLeaves = 0; // Reset for incoming walk mSource->Walk(Local::SetupLeafData, &Data); // Handle source indices { bool MustKeepIndices = true; if(create.mCanRemap) { // We try to get rid of source indices (saving more ram!) by reorganizing triangle arrays... // Remap can fail when we use callbacks => keep track of indices in that case (it still // works, only using more memory) if(create.mIMesh->RemapClient(mSource->GetNbPrimitives(), mSource->GetIndices())) { MustKeepIndices = false; } } if(MustKeepIndices) { // Keep track of source indices (from vanilla tree) mNbPrimitives = mSource->GetNbPrimitives(); mIndices = new udword[mNbPrimitives]; CopyMemory(mIndices, mSource->GetIndices(), mNbPrimitives*sizeof(udword)); } } // Now, create our optimized tree using previous leaf nodes LeafTree = new AABBTree; CHECKALLOC(LeafTree); { AABBTreeOfAABBsBuilder TB; // Now using boxes ! TB.mSettings = create.mSettings; TB.mSettings.mLimit = 1; // We now want a complete tree so that we can "optimize" it TB.mNbPrimitives = Data.mNbLeaves; TB.mAABBArray = Data.mLeaves; if(!LeafTree->Build(&TB)) goto FreeAndExit; } // 3) Create an optimized tree according to user-settings if(!CreateTree(create.mNoLeaf, create.mQuantized)) goto FreeAndExit; // 3-2) Create optimized tree if(!mTree->Build(LeafTree)) goto FreeAndExit; // Finally ok... Status = true; FreeAndExit: // Allow me this one... DELETESINGLE(LeafTree); // 3-3) Delete generic tree if needed if(!create.mKeepOriginal) DELETESINGLE(mSource); return Status; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of bytes used by the tree. * \return amount of bytes used */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword HybridModel::GetUsedBytes() const { udword UsedBytes = 0; if(mTree) UsedBytes += mTree->GetUsedBytes(); if(mIndices) UsedBytes += mNbPrimitives * sizeof(udword); // mIndices if(mTriangles) UsedBytes += mNbLeaves * sizeof(LeafTriangles); // mTriangles return UsedBytes; } inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp) { // Compute triangle's AABB = a leaf box #ifdef OPC_USE_FCOMI // a 15% speedup on my machine, not much min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); #else min = *vp.Vertex[0]; max = *vp.Vertex[0]; min.Min(*vp.Vertex[1]); max.Max(*vp.Vertex[1]); min.Min(*vp.Vertex[2]); max.Max(*vp.Vertex[2]); #endif } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the collision model. This can be used to handle dynamic meshes. Usage is: * 1. modify your mesh vertices (keep the topology constant!) * 2. refit the tree (call this method) * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool HybridModel::Refit() { if(!mIMesh) return false; if(!mTree) return false; if(IsQuantized()) return false; if(HasLeafNodes()) return false; const LeafTriangles* LT = GetLeafTriangles(); const udword* Indices = GetIndices(); // Bottom-up update VertexPointers VP; Point Min,Max; Point Min_,Max_; udword Index = mTree->GetNbNodes(); AABBNoLeafNode* Nodes = (AABBNoLeafNode*)((AABBNoLeafTree*)mTree)->GetNodes(); while(Index--) { AABBNoLeafNode& Current = Nodes[Index]; if(Current.HasPosLeaf()) { const LeafTriangles& CurrentLeaf = LT[Current.GetPosPrimitive()]; Min.SetPlusInfinity(); Max.SetMinusInfinity(); Point TmpMin, TmpMax; // Each leaf box has a set of triangles udword NbTris = CurrentLeaf.GetNbTriangles(); if(Indices) { const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; // Loop through triangles and test each of them while(NbTris--) { mIMesh->GetTriangle(VP, *T++); ComputeMinMax(TmpMin, TmpMax, VP); Min.Min(TmpMin); Max.Max(TmpMax); } } else { udword BaseIndex = CurrentLeaf.GetTriangleIndex(); // Loop through triangles and test each of them while(NbTris--) { mIMesh->GetTriangle(VP, BaseIndex++); ComputeMinMax(TmpMin, TmpMax, VP); Min.Min(TmpMin); Max.Max(TmpMax); } } } else { const CollisionAABB& CurrentBox = Current.GetPos()->mAABB; CurrentBox.GetMin(Min); CurrentBox.GetMax(Max); } if(Current.HasNegLeaf()) { const LeafTriangles& CurrentLeaf = LT[Current.GetNegPrimitive()]; Min_.SetPlusInfinity(); Max_.SetMinusInfinity(); Point TmpMin, TmpMax; // Each leaf box has a set of triangles udword NbTris = CurrentLeaf.GetNbTriangles(); if(Indices) { const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; // Loop through triangles and test each of them while(NbTris--) { mIMesh->GetTriangle(VP, *T++); ComputeMinMax(TmpMin, TmpMax, VP); Min_.Min(TmpMin); Max_.Max(TmpMax); } } else { udword BaseIndex = CurrentLeaf.GetTriangleIndex(); // Loop through triangles and test each of them while(NbTris--) { mIMesh->GetTriangle(VP, BaseIndex++); ComputeMinMax(TmpMin, TmpMax, VP); Min_.Min(TmpMin); Max_.Max(TmpMax); } } } else { const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB; CurrentBox.GetMin(Min_); CurrentBox.GetMax(Max_); } #ifdef OPC_USE_FCOMI Min.x = FCMin2(Min.x, Min_.x); Max.x = FCMax2(Max.x, Max_.x); Min.y = FCMin2(Min.y, Min_.y); Max.y = FCMax2(Max.y, Max_.y); Min.z = FCMin2(Min.z, Min_.z); Max.z = FCMax2(Max.z, Max_.z); #else Min.Min(Min_); Max.Max(Max_); #endif Current.mAABB.SetMinMax(Min, Max); } return true; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_HybridModel.h000066400000000000000000000141371207742442300236030ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for hybrid models. * \file OPC_HybridModel.h * \author Pierre Terdiman * \date May, 18, 2003 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_HYBRIDMODEL_H__ #define __OPC_HYBRIDMODEL_H__ //! Leaf descriptor struct LeafTriangles { udword Data; //!< Packed data /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets number of triangles in the leaf. * \return number of triangles N, with 0 < N <= 16 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbTriangles() const { return (Data & 15)+1; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets triangle index for this leaf. Indexed model's array of indices retrieved with HybridModel::GetIndices() * \return triangle index */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetTriangleIndex() const { return Data>>4; } inline_ void SetData(udword nb, udword index) { ASSERT(nb>0 && nb<=16); nb--; Data = (index<<4)|(nb&15); } }; class OPCODE_API HybridModel : public BaseModel { public: // Constructor/Destructor HybridModel(); virtual ~HybridModel(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds a collision model. * \param create [in] model creation structure * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(BaseModel) bool Build(const OPCODECREATE& create); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of bytes used by the tree. * \return amount of bytes used */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(BaseModel) udword GetUsedBytes() const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the collision model. This can be used to handle dynamic meshes. Usage is: * 1. modify your mesh vertices (keep the topology constant!) * 2. refit the tree (call this method) * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(BaseModel) bool Refit(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets array of triangles. * \return array of triangles */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ const LeafTriangles* GetLeafTriangles() const { return mTriangles; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets array of indices. * \return array of indices */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ const udword* GetIndices() const { return mIndices; } private: udword mNbLeaves; //!< Number of leaf nodes in the model LeafTriangles* mTriangles; //!< Array of mNbLeaves leaf descriptors udword mNbPrimitives; //!< Number of primitives in the model udword* mIndices; //!< Array of primitive indices // Internal methods void Release(); }; #endif // __OPC_HYBRIDMODEL_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_IceHook.h000066400000000000000000000030211207742442300227100ustar00rootroot00000000000000 // Should be included by Opcode.h if needed #define ICE_DONT_CHECK_COMPILER_OPTIONS // From Windows... typedef int BOOL; #ifndef FALSE #define FALSE 0 #endif #ifndef TRUE #define TRUE 1 #endif #include #include #include #include #include #include #ifndef ASSERT #define ASSERT(exp) {} #endif #define ICE_COMPILE_TIME_ASSERT(exp) extern char ICE_Dummy[ (exp) ? 1 : -1 ] #define Log {} #define SetIceError(a,b) false #define EC_OUTOFMEMORY "Out of memory" #include "Ice/IcePreprocessor.h" #undef ICECORE_API #define ICECORE_API OPCODE_API #include "Ice/IceTypes.h" #include "Ice/IceFPU.h" #include "Ice/IceMemoryMacros.h" namespace IceCore { #include "Ice/IceUtils.h" #include "Ice/IceContainer.h" #include "Ice/IcePairs.h" #include "Ice/IceRevisitedRadix.h" #include "Ice/IceRandom.h" } using namespace IceCore; #define ICEMATHS_API OPCODE_API namespace IceMaths { #include "Ice/IceAxes.h" #include "Ice/IcePoint.h" #include "Ice/IceHPoint.h" #include "Ice/IceMatrix3x3.h" #include "Ice/IceMatrix4x4.h" #include "Ice/IcePlane.h" #include "Ice/IceRay.h" #include "Ice/IceIndexedTriangle.h" //#include "Ice/IceTriangle.h" //#include "Ice/IceTriList.h" #include "Ice/IceAABB.h" //#include "Ice/IceOBB.h" #include "Ice/IceBoundingSphere.h" //#include "Ice/IceSegment.h" //#include "Ice/IceLSS.h" } using namespace IceMaths; choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_MeshInterface.cpp000066400000000000000000000327361207742442300244560ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a mesh interface. * \file OPC_MeshInterface.cpp * \author Pierre Terdiman * \date November, 27, 2002 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * This structure holds 3 vertex-pointers. It's mainly used by collision callbacks so that the app doesn't have * to return 3 vertices to OPCODE (36 bytes) but only 3 pointers (12 bytes). It seems better but I never profiled * the alternative. * * \class VertexPointers * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * This class is an interface between us and user-defined meshes. Meshes can be defined in a lot of ways, and here we * try to support most of them. * * Basically you have two options: * - callbacks, if OPC_USE_CALLBACKS is defined in OPC_Settings.h. * - else pointers. * * If using pointers, you can also use strides or not. Strides are used when OPC_USE_STRIDE is defined. * * * CALLBACKS: * * Using callbacks is the most generic way to feed OPCODE with your meshes. Indeed, you just have to give * access to three vertices at the end of the day. It's up to you to fetch them from your database, using * whatever method you want. Hence your meshes can lie in system memory or AGP, be indexed or not, use 16 * or 32-bits indices, you can decompress them on-the-fly if needed, etc. On the other hand, a callback is * called each time OPCODE needs access to a particular triangle, so there might be a slight overhead. * * To make things clear: geometry & topology are NOT stored in the collision system, * in order to save some ram. So, when the system needs them to perform accurate intersection * tests, you're requested to provide the triangle-vertices corresponding to a given face index. * * Ex: * * \code * static void ColCallback(udword triangle_index, VertexPointers& triangle, udword user_data) * { * // Get back Mesh0 or Mesh1 (you also can use 2 different callbacks) * Mesh* MyMesh = (Mesh*)user_data; * // Get correct triangle in the app-controlled database * const Triangle* Tri = MyMesh->GetTriangle(triangle_index); * // Setup pointers to vertices for the collision system * triangle.Vertex[0] = MyMesh->GetVertex(Tri->mVRef[0]); * triangle.Vertex[1] = MyMesh->GetVertex(Tri->mVRef[1]); * triangle.Vertex[2] = MyMesh->GetVertex(Tri->mVRef[2]); * } * * // Setup callbacks * MeshInterface0->SetCallback(ColCallback, udword(Mesh0)); * MeshInterface1->SetCallback(ColCallback, udword(Mesh1)); * \endcode * * Of course, you should make this callback as fast as possible. And you're also not supposed * to modify the geometry *after* the collision trees have been built. The alternative was to * store the geometry & topology in the collision system as well (as in RAPID) but we have found * this approach to waste a lot of ram in many cases. * * * POINTERS: * * If you're internally using the following canonical structures: * - a vertex made of three 32-bits floating point values * - a triangle made of three 32-bits integer vertex references * ...then you may want to use pointers instead of callbacks. This is the same, except OPCODE will directly * use provided pointers to access the topology and geometry, without using a callback. It might be faster, * but probably not as safe. Pointers have been introduced in OPCODE 1.2. * * Ex: * * \code * // Setup pointers * MeshInterface0->SetPointers(Mesh0->GetFaces(), Mesh0->GetVerts()); * MeshInterface1->SetPointers(Mesh1->GetFaces(), Mesh1->GetVerts()); * \endcode * * * STRIDES: * * If your vertices are D3D-like entities interleaving a position, a normal and/or texture coordinates * (i.e. if your vertices are FVFs), you might want to use a vertex stride to skip extra data OPCODE * doesn't need. Using a stride shouldn't be notably slower than not using it, but it might increase * cache misses. Please also note that you *shouldn't* read from AGP or video-memory buffers ! * * * In any case, compilation flags are here to select callbacks/pointers/strides at compile time, so * choose what's best for your application. All of this has been wrapped into this MeshInterface. * * \class MeshInterface * \author Pierre Terdiman * \version 1.3 * \date November, 27, 2002 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; Point MeshInterface::VertexCache[3]; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// MeshInterface::MeshInterface() : mNbTris (0), mNbVerts (0), #ifdef OPC_USE_CALLBACKS mUserData (null), mObjCallback (null), #else mTris (null), mVerts (null), #ifdef OPC_USE_STRIDE mTriStride (sizeof(IndexedTriangle)), mVertexStride (sizeof(Point)), #endif #endif Single(true) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// MeshInterface::~MeshInterface() { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the mesh interface is valid, i.e. things have been setup correctly. * \return true if valid */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool MeshInterface::IsValid() const { if(!mNbTris || !mNbVerts) return false; #ifdef OPC_USE_CALLBACKS if(!mObjCallback) return false; #else if(!mTris || !mVerts) return false; #endif return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the mesh itself is valid. * Currently we only look for degenerate faces. * \return number of degenerate faces */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword MeshInterface::CheckTopology() const { // Check topology. If the model contains degenerate faces, collision report can be wrong in some cases. // e.g. it happens with the standard MAX teapot. So clean your meshes first... If you don't have a mesh cleaner // you can try this: www.codercorner.com/Consolidation.zip udword NbDegenerate = 0; VertexPointers VP; // Using callbacks, we don't have access to vertex indices. Nevertheless we still can check for // redundant vertex pointers, which cover all possibilities (callbacks/pointers/strides). for(udword i=0;i= 0.0f; } }; #ifdef OPC_USE_CALLBACKS /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * User-callback, called by OPCODE to request vertices from the app. * \param triangle_index [in] face index for which the system is requesting the vertices * \param triangle [out] triangle's vertices (must be provided by the user) * \param user_data [in] user-defined data from SetCallback() */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// typedef void (*RequestCallback) (udword triangle_index, VertexPointers& triangle, void* user_data); #endif class OPCODE_API MeshInterface { public: // Constructor / Destructor MeshInterface(); ~MeshInterface(); // Common settings inline_ udword GetNbTriangles() const { return mNbTris; } inline_ udword GetNbVertices() const { return mNbVerts; } inline_ void SetNbTriangles(udword nb) { mNbTris = nb; } inline_ void SetNbVertices(udword nb) { mNbVerts = nb; } #ifdef OPC_USE_CALLBACKS // Callback settings /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Callback control: setups object callback. Must provide triangle-vertices for a given triangle index. * \param callback [in] user-defined callback * \param user_data [in] user-defined data * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool SetCallback(RequestCallback callback, void* user_data); inline_ void* GetUserData() const { return mUserData; } inline_ RequestCallback GetCallback() const { return mObjCallback; } #else // Pointers settings /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Pointers control: setups object pointers. Must provide access to faces and vertices for a given object. * \param tris [in] pointer to triangles * \param verts [in] pointer to vertices * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool SetPointers(const IndexedTriangle* tris, const Point* verts); inline_ const IndexedTriangle* GetTris() const { return mTris; } inline_ const Point* GetVerts() const { return mVerts; } #ifdef OPC_USE_STRIDE // Strides settings /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Strides control * \param tri_stride [in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices. * \param vertex_stride [in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position. * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool SetStrides(udword tri_stride=sizeof(IndexedTriangle), udword vertex_stride=sizeof(Point)); inline_ udword GetTriStride() const { return mTriStride; } inline_ udword GetVertexStride() const { return mVertexStride; } #endif #endif /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Fetches a triangle given a triangle index. * \param vp [out] required triangle's vertex pointers * \param index [in] triangle index */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void GetTriangle(VertexPointers& vp, udword index) const { #ifdef OPC_USE_CALLBACKS (mObjCallback)(index, vp, mUserData); #else #ifdef OPC_USE_STRIDE const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + index * mTriStride); if (Single){ vp.Vertex[0] = (const Point*)(((ubyte*)mVerts) + T->mVRef[0] * mVertexStride); vp.Vertex[1] = (const Point*)(((ubyte*)mVerts) + T->mVRef[1] * mVertexStride); vp.Vertex[2] = (const Point*)(((ubyte*)mVerts) + T->mVRef[2] * mVertexStride); } else{ for (int i = 0; i < 3; i++){ const double* v = (const double*)(((ubyte*)mVerts) + T->mVRef[i] * mVertexStride); VertexCache[i].x = (float)v[0]; VertexCache[i].y = (float)v[1]; VertexCache[i].z = (float)v[2]; vp.Vertex[i] = &VertexCache[i]; } } #else const IndexedTriangle* T = &mTris[index]; vp.Vertex[0] = &mVerts[T->mVRef[0]]; vp.Vertex[1] = &mVerts[T->mVRef[1]]; vp.Vertex[2] = &mVerts[T->mVRef[2]]; #endif #endif } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Remaps client's mesh according to a permutation. * \param nb_indices [in] number of indices in the permutation (will be checked against number of triangles) * \param permutation [in] list of triangle indices * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool RemapClient(udword nb_indices, const udword* permutation) const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the mesh interface is valid, i.e. things have been setup correctly. * \return true if valid */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool IsValid() const; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the mesh itself is valid. * Currently we only look for degenerate faces. * \return number of degenerate faces */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword CheckTopology() const; private: udword mNbTris; //!< Number of triangles in the input model udword mNbVerts; //!< Number of vertices in the input model #ifdef OPC_USE_CALLBACKS // User callback void* mUserData; //!< User-defined data sent to callback RequestCallback mObjCallback; //!< Object callback #else // User pointers const IndexedTriangle* mTris; //!< Array of indexed triangles const Point* mVerts; //!< Array of vertices #ifdef OPC_USE_STRIDE udword mTriStride; //!< Possible triangle stride in bytes [Opcode 1.3] udword mVertexStride; //!< Possible vertex stride in bytes [Opcode 1.3] #endif public: bool Single; //!< Use single or double precision vertices private: static Point VertexCache[3]; #endif }; #endif //__OPC_MESHINTERFACE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Model.cpp000066400000000000000000000206561207742442300227770ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for OPCODE models. * \file OPC_Model.cpp * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * The main collision wrapper, for all trees. Supported trees are: * - Normal trees (2*N-1 nodes, full size) * - No-leaf trees (N-1 nodes, full size) * - Quantized trees (2*N-1 nodes, half size) * - Quantized no-leaf trees (N-1 nodes, half size) * * Usage: * * 1) Create a static mesh interface using callbacks or pointers. (see OPC_MeshInterface.cpp). * Keep it around in your app, since a pointer to this interface is saved internally and * used until you release the collision structures. * * 2) Build a Model using a creation structure: * * \code * Model Sample; * * OPCODECREATE OPCC; * OPCC.IMesh = ...; * OPCC.Rules = ...; * OPCC.NoLeaf = ...; * OPCC.Quantized = ...; * OPCC.KeepOriginal = ...; * bool Status = Sample.Build(OPCC); * \endcode * * 3) Create a tree collider and set it up: * * \code * AABBTreeCollider TC; * TC.SetFirstContact(...); * TC.SetFullBoxBoxTest(...); * TC.SetFullPrimBoxTest(...); * TC.SetTemporalCoherence(...); * \endcode * * 4) Perform a collision query * * \code * // Setup cache * static BVTCache ColCache; * ColCache.Model0 = &Model0; * ColCache.Model1 = &Model1; * * // Collision query * bool IsOk = TC.Collide(ColCache, World0, World1); * * // Get collision status => if true, objects overlap * BOOL Status = TC.GetContactStatus(); * * // Number of colliding pairs and list of pairs * udword NbPairs = TC.GetNbPairs(); * const Pair* p = TC.GetPairs() * \endcode * * 5) Stats * * \code * Model0.GetUsedBytes() = number of bytes used for this collision tree * TC.GetNbBVBVTests() = number of BV-BV overlap tests performed during last query * TC.GetNbPrimPrimTests() = number of Triangle-Triangle overlap tests performed during last query * TC.GetNbBVPrimTests() = number of Triangle-BV overlap tests performed during last query * \endcode * * \class Model * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Model::Model() { #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! mHull = null; #endif // __MESHMERIZER_H__ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Model::~Model() { Release(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Releases the model. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Model::Release() { ReleaseBase(); #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! DELETESINGLE(mHull); #endif // __MESHMERIZER_H__ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds a collision model. * \param create [in] model creation structure * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Model::Build(const OPCODECREATE& create) { // 1) Checkings if(!create.mIMesh || !create.mIMesh->IsValid()) return false; // For this model, we only support complete trees if(create.mSettings.mLimit!=1) return SetIceError("OPCODE WARNING: supports complete trees only! Use mLimit = 1.\n", null); // Look for degenerate faces. udword NbDegenerate = create.mIMesh->CheckTopology(); if(NbDegenerate) Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate); // We continue nonetheless.... Release(); // Make sure previous tree has been discarded [Opcode 1.3, thanks Adam] // 1-1) Setup mesh interface automatically [Opcode 1.3] SetMeshInterface(create.mIMesh); // Special case for 1-triangle meshes [Opcode 1.3] udword NbTris = create.mIMesh->GetNbTriangles(); if(NbTris==1) { // We don't need to actually create a tree here, since we'll only have a single triangle to deal with anyway. // It's a waste to use a "model" for this but at least it will work. mModelCode |= OPC_SINGLE_NODE; return true; } // 2) Build a generic AABB Tree. mSource = new AABBTree; CHECKALLOC(mSource); // 2-1) Setup a builder. Our primitives here are triangles from input mesh, // so we use an AABBTreeOfTrianglesBuilder..... { AABBTreeOfTrianglesBuilder TB; TB.mIMesh = create.mIMesh; TB.mSettings = create.mSettings; TB.mNbPrimitives = NbTris; if(!mSource->Build(&TB)) return false; } // 3) Create an optimized tree according to user-settings if(!CreateTree(create.mNoLeaf, create.mQuantized)) return false; // 3-2) Create optimized tree if(!mTree->Build(mSource)) return false; // 3-3) Delete generic tree if needed if(!create.mKeepOriginal) DELETESINGLE(mSource); #ifdef __MESHMERIZER_H__ // 4) Convex hull if(create.mCollisionHull) { // Create hull mHull = new CollisionHull; CHECKALLOC(mHull); CONVEXHULLCREATE CHC; // ### doesn't work with strides CHC.NbVerts = create.mIMesh->GetNbVertices(); CHC.Vertices = create.mIMesh->GetVerts(); CHC.UnifyNormals = true; CHC.ReduceVertices = true; CHC.WordFaces = false; mHull->Compute(CHC); } #endif // __MESHMERIZER_H__ return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of bytes used by the tree. * \return amount of bytes used */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// udword Model::GetUsedBytes() const { if(!mTree) return 0; return mTree->GetUsedBytes(); } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Model.h000066400000000000000000000065361207742442300224450ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for OPCODE models. * \file OPC_Model.h * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_MODEL_H__ #define __OPC_MODEL_H__ class OPCODE_API Model : public BaseModel { public: // Constructor/Destructor Model(); virtual ~Model(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds a collision model. * \param create [in] model creation structure * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(BaseModel) bool Build(const OPCODECREATE& create); #ifdef __MESHMERIZER_H__ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the collision hull. * \return the collision hull if it exists */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ const CollisionHull* GetHull() const { return mHull; } #endif // __MESHMERIZER_H__ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of bytes used by the tree. * \return amount of bytes used */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(BaseModel) udword GetUsedBytes() const; private: #ifdef __MESHMERIZER_H__ CollisionHull* mHull; //!< Possible convex hull #endif // __MESHMERIZER_H__ // Internal methods void Release(); }; #endif //__OPC_MODEL_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_OptimizedTree.cpp000066400000000000000000001012151207742442300245120ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for optimized trees. Implements 4 trees: * - normal * - no leaf * - quantized * - no leaf / quantized * * \file OPC_OptimizedTree.cpp * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A standard AABB tree. * * \class AABBCollisionTree * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A no-leaf AABB tree. * * \class AABBNoLeafTree * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A quantized AABB tree. * * \class AABBQuantizedTree * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A quantized no-leaf AABB tree. * * \class AABBQuantizedNoLeafTree * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; //! Compilation flag: //! - true to fix quantized boxes (i.e. make sure they enclose the original ones) //! - false to see the effects of quantization errors (faster, but wrong results in some cases) static bool gFixQuantized = true; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds an implicit tree from a standard one. An implicit tree is a complete tree (2*N-1 nodes) whose negative * box pointers and primitive pointers have been made implicit, hence packing 3 pointers in one. * * Layout for implicit trees: * Node: * - box * - data (32-bits value) * * if data's LSB = 1 => remaining bits are a primitive pointer * else remaining bits are a P-node pointer, and N = P + 1 * * \relates AABBCollisionNode * \fn _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) * \param linear [in] base address of destination nodes * \param box_id [in] index of destination node * \param current_id [in] current running index * \param current_node [in] current node from input tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) { // Current node from input tree is "current_node". Must be flattened into "linear[boxid]". // Store the AABB current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter); current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents); #if 1 // Added by AIST linear[box_id].mAABB.CreateSSV(); #endif // Store remaining info if(current_node->IsLeaf()) { // The input tree must be complete => i.e. one primitive/leaf ASSERT(current_node->GetNbPrimitives()==1); // Get the primitive index from the input tree udword PrimitiveIndex = current_node->GetPrimitives()[0]; // Setup box data as the primitive index, marked as leaf linear[box_id].mData = (PrimitiveIndex<<1)|1; } else { // To make the negative one implicit, we must store P and N in successive order udword PosID = current_id++; // Get a new id for positive child udword NegID = current_id++; // Get a new id for negative child // Setup box data as the forthcoming new P pointer linear[box_id].mData = (EXWORD)&linear[PosID]; // Make sure it's not marked as leaf ASSERT(!(linear[box_id].mData&1)); //Modified by S-cubed, Inc. //examine_normal_vector@InsertCollisionPair.cpp $B$G?F$rDI$($k$h$&$K$9$k(B ((AABBCollisionNode*)linear[box_id].mData)->mB = &linear[box_id]; (((AABBCollisionNode*)linear[box_id].mData)+1)->mB = &linear[box_id]; // Recurse with new IDs _BuildCollisionTree(linear, PosID, current_id, current_node->GetPos()); _BuildCollisionTree(linear, NegID, current_id, current_node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds a "no-leaf" tree from a standard one. This is a tree whose leaf nodes have been removed. * * Layout for no-leaf trees: * * Node: * - box * - P pointer => a node (LSB=0) or a primitive (LSB=1) * - N pointer => a node (LSB=0) or a primitive (LSB=1) * * \relates AABBNoLeafNode * \fn _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) * \param linear [in] base address of destination nodes * \param box_id [in] index of destination node * \param current_id [in] current running index * \param current_node [in] current node from input tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// static void _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) { const AABBTreeNode* P = current_node->GetPos(); const AABBTreeNode* N = current_node->GetNeg(); // Leaf nodes here?! ASSERT(P); ASSERT(N); // Internal node => keep the box current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter); current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents); if(P->IsLeaf()) { // The input tree must be complete => i.e. one primitive/leaf ASSERT(P->GetNbPrimitives()==1); // Get the primitive index from the input tree udword PrimitiveIndex = P->GetPrimitives()[0]; // Setup prev box data as the primitive index, marked as leaf linear[box_id].mPosData = (PrimitiveIndex<<1)|1; } else { // Get a new id for positive child udword PosID = current_id++; // Setup box data linear[box_id].mPosData = (EXWORD)&linear[PosID]; // Make sure it's not marked as leaf ASSERT(!(linear[box_id].mPosData&1)); // Recurse _BuildNoLeafTree(linear, PosID, current_id, P); } if(N->IsLeaf()) { // The input tree must be complete => i.e. one primitive/leaf ASSERT(N->GetNbPrimitives()==1); // Get the primitive index from the input tree udword PrimitiveIndex = N->GetPrimitives()[0]; // Setup prev box data as the primitive index, marked as leaf linear[box_id].mNegData = (PrimitiveIndex<<1)|1; } else { // Get a new id for negative child udword NegID = current_id++; // Setup box data linear[box_id].mNegData = (EXWORD)&linear[NegID]; // Make sure it's not marked as leaf ASSERT(!(linear[box_id].mNegData&1)); // Recurse _BuildNoLeafTree(linear, NegID, current_id, N); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBCollisionTree::AABBCollisionTree() : mNodes(null) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBCollisionTree::~AABBCollisionTree() { DELETEARRAY(mNodes); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds the collision tree from a generic AABB tree. * \param tree [in] generic AABB tree * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBCollisionTree::Build(AABBTree* tree) { // Checkings if(!tree) return false; // Check the input tree is complete udword NbTriangles = tree->GetNbPrimitives(); udword NbNodes = tree->GetNbNodes(); if(NbNodes!=NbTriangles*2-1) return false; // Get nodes if(mNbNodes!=NbNodes) // Same number of nodes => keep moving { mNbNodes = NbNodes; DELETEARRAY(mNodes); mNodes = new AABBCollisionNode[mNbNodes]; CHECKALLOC(mNodes); } // Build the tree udword CurID = 1; // Modified by S-cubed, Inc. //$B%k!<%H$N?F$O<+J,<+?H(B mNodes[0].mB = &mNodes[0]; _BuildCollisionTree(mNodes, 0, CurID, tree); ASSERT(CurID==mNbNodes); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the collision tree after vertices have been modified. * \param mesh_interface [in] mesh interface for current model * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBCollisionTree::Refit(const MeshInterface* mesh_interface) { ASSERT(!"Not implemented since AABBCollisionTrees have twice as more nodes to refit as AABBNoLeafTrees!"); return false; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Walks the tree and call the user back for each node. * \param callback [in] walking callback * \param user_data [in] callback's user data * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) const { if(!callback) return false; struct Local { static void _Walk(const AABBCollisionNode* current_node, GenericWalkingCallback callback, void* user_data) { if(!current_node || !(callback)(current_node, user_data)) return; if(!current_node->IsLeaf()) { _Walk(current_node->GetPos(), callback, user_data); _Walk(current_node->GetNeg(), callback, user_data); } } }; Local::_Walk(mNodes, callback, user_data); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBNoLeafTree::AABBNoLeafTree() : mNodes(null) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBNoLeafTree::~AABBNoLeafTree() { DELETEARRAY(mNodes); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds the collision tree from a generic AABB tree. * \param tree [in] generic AABB tree * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBNoLeafTree::Build(AABBTree* tree) { // Checkings if(!tree) return false; // Check the input tree is complete udword NbTriangles = tree->GetNbPrimitives(); udword NbNodes = tree->GetNbNodes(); if(NbNodes!=NbTriangles*2-1) return false; // Get nodes if(mNbNodes!=NbTriangles-1) // Same number of nodes => keep moving { mNbNodes = NbTriangles-1; DELETEARRAY(mNodes); mNodes = new AABBNoLeafNode[mNbNodes]; CHECKALLOC(mNodes); } // Build the tree udword CurID = 1; _BuildNoLeafTree(mNodes, 0, CurID, tree); ASSERT(CurID==mNbNodes); return true; } inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp) { // Compute triangle's AABB = a leaf box #ifdef OPC_USE_FCOMI // a 15% speedup on my machine, not much min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); #else min = *vp.Vertex[0]; max = *vp.Vertex[0]; min.Min(*vp.Vertex[1]); max.Max(*vp.Vertex[1]); min.Min(*vp.Vertex[2]); max.Max(*vp.Vertex[2]); #endif } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the collision tree after vertices have been modified. * \param mesh_interface [in] mesh interface for current model * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface) { // Checkings if(!mesh_interface) return false; // Bottom-up update VertexPointers VP; Point Min,Max; Point Min_,Max_; udword Index = mNbNodes; while(Index--) { AABBNoLeafNode& Current = mNodes[Index]; if(Current.HasPosLeaf()) { mesh_interface->GetTriangle(VP, Current.GetPosPrimitive()); ComputeMinMax(Min, Max, VP); } else { const CollisionAABB& CurrentBox = Current.GetPos()->mAABB; CurrentBox.GetMin(Min); CurrentBox.GetMax(Max); } if(Current.HasNegLeaf()) { mesh_interface->GetTriangle(VP, Current.GetNegPrimitive()); ComputeMinMax(Min_, Max_, VP); } else { const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB; CurrentBox.GetMin(Min_); CurrentBox.GetMax(Max_); } #ifdef OPC_USE_FCOMI Min.x = FCMin2(Min.x, Min_.x); Max.x = FCMax2(Max.x, Max_.x); Min.y = FCMin2(Min.y, Min_.y); Max.y = FCMax2(Max.y, Max_.y); Min.z = FCMin2(Min.z, Min_.z); Max.z = FCMax2(Max.z, Max_.z); #else Min.Min(Min_); Max.Max(Max_); #endif Current.mAABB.SetMinMax(Min, Max); } return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Walks the tree and call the user back for each node. * \param callback [in] walking callback * \param user_data [in] callback's user data * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const { if(!callback) return false; struct Local { static void _Walk(const AABBNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data) { if(!current_node || !(callback)(current_node, user_data)) return; if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data); if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data); } }; Local::_Walk(mNodes, callback, user_data); return true; } // Quantization notes: // - We could use the highest bits of mData to store some more quantized bits. Dequantization code // would be slightly more complex, but number of overlap tests would be reduced (and anyhow those // bits are currently wasted). Of course it's not possible if we move to 16 bits mData. // - Something like "16 bits floats" could be tested, to bypass the int-to-float conversion. // - A dedicated BV-BV test could be used, dequantizing while testing for overlap. (i.e. it's some // lazy-dequantization which may save some work in case of early exits). At the very least some // muls could be saved by precomputing several more matrices. But maybe not worth the pain. // - Do we need to dequantize anyway? Not doing the extents-related muls only implies the box has // been scaled, for example. // - The deeper we move into the hierarchy, the smaller the extents should be. May not need a fixed // number of quantization bits. Even better, could probably be best delta-encoded. // Find max values. Some people asked why I wasn't simply using the first node. Well, I can't. // I'm not looking for (min, max) values like in a standard AABB, I'm looking for the extremal // centers/extents in order to quantize them. The first node would only give a single center and // a single extents. While extents would be the biggest, the center wouldn't. #define FIND_MAX_VALUES \ /* Get max values */ \ Point CMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); \ Point EMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); \ for(udword i=0;iCMax.x) CMax.x = fabsf(Nodes[i].mAABB.mCenter.x); \ if(fabsf(Nodes[i].mAABB.mCenter.y)>CMax.y) CMax.y = fabsf(Nodes[i].mAABB.mCenter.y); \ if(fabsf(Nodes[i].mAABB.mCenter.z)>CMax.z) CMax.z = fabsf(Nodes[i].mAABB.mCenter.z); \ if(fabsf(Nodes[i].mAABB.mExtents.x)>EMax.x) EMax.x = fabsf(Nodes[i].mAABB.mExtents.x); \ if(fabsf(Nodes[i].mAABB.mExtents.y)>EMax.y) EMax.y = fabsf(Nodes[i].mAABB.mExtents.y); \ if(fabsf(Nodes[i].mAABB.mExtents.z)>EMax.z) EMax.z = fabsf(Nodes[i].mAABB.mExtents.z); \ } #define INIT_QUANTIZATION \ udword nbc=15; /* Keep one bit for sign */ \ udword nbe=15; /* Keep one bit for fix */ \ if(!gFixQuantized) nbe++; \ \ /* Compute quantization coeffs */ \ Point CQuantCoeff, EQuantCoeff; \ CQuantCoeff.x = CMax.x!=0.0f ? float((1<Min[j]) mNodes[i].mAABB.mExtents[j]++; \ else FixMe=false; \ /* Prevent wrapping */ \ if(!mNodes[i].mAABB.mExtents[j]) \ { \ mNodes[i].mAABB.mExtents[j]=0xffff; \ FixMe=false; \ } \ }while(FixMe); \ } \ } #ifdef __x86_64 #define REMAP_DATA(member) \ /* Fix data */ \ Data = Nodes[i].member; \ if(!(Data&1)) \ { \ /* Compute box number */ \ uqword Nb = (Data - uqword(Nodes))/Nodes[i].GetNodeSize(); \ Data = uqword(&mNodes[Nb]); \ } \ /* ...remapped */ \ mNodes[i].member = Data; #else #define REMAP_DATA(member) \ /* Fix data */ \ Data = Nodes[i].member; \ if(!(Data&1)) \ { \ /* Compute box number */ \ udword Nb = (Data - udword(Nodes))/Nodes[i].GetNodeSize(); \ Data = udword(&mNodes[Nb]); \ } \ /* ...remapped */ \ mNodes[i].member = Data; #endif /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBQuantizedTree::AABBQuantizedTree() : mNodes(null) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBQuantizedTree::~AABBQuantizedTree() { DELETEARRAY(mNodes); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds the collision tree from a generic AABB tree. * \param tree [in] generic AABB tree * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBQuantizedTree::Build(AABBTree* tree) { // Checkings if(!tree) return false; // Check the input tree is complete udword NbTriangles = tree->GetNbPrimitives(); udword NbNodes = tree->GetNbNodes(); if(NbNodes!=NbTriangles*2-1) return false; // Get nodes mNbNodes = NbNodes; DELETEARRAY(mNodes); AABBCollisionNode* Nodes = new AABBCollisionNode[mNbNodes]; CHECKALLOC(Nodes); // Build the tree udword CurID = 1; _BuildCollisionTree(Nodes, 0, CurID, tree); // Quantize { mNodes = new AABBQuantizedNode[mNbNodes]; CHECKALLOC(mNodes); // Get max values FIND_MAX_VALUES // Quantization INIT_QUANTIZATION // Quantize EXWORD Data; for(udword i=0;iIsLeaf()) { _Walk(current_node->GetPos(), callback, user_data); _Walk(current_node->GetNeg(), callback, user_data); } } }; Local::_Walk(mNodes, callback, user_data); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBQuantizedNoLeafTree::AABBQuantizedNoLeafTree() : mNodes(null) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBQuantizedNoLeafTree::~AABBQuantizedNoLeafTree() { DELETEARRAY(mNodes); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds the collision tree from a generic AABB tree. * \param tree [in] generic AABB tree * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBQuantizedNoLeafTree::Build(AABBTree* tree) { // Checkings if(!tree) return false; // Check the input tree is complete udword NbTriangles = tree->GetNbPrimitives(); udword NbNodes = tree->GetNbNodes(); if(NbNodes!=NbTriangles*2-1) return false; // Get nodes mNbNodes = NbTriangles-1; DELETEARRAY(mNodes); AABBNoLeafNode* Nodes = new AABBNoLeafNode[mNbNodes]; CHECKALLOC(Nodes); // Build the tree udword CurID = 1; _BuildNoLeafTree(Nodes, 0, CurID, tree); ASSERT(CurID==mNbNodes); // Quantize { mNodes = new AABBQuantizedNoLeafNode[mNbNodes]; CHECKALLOC(mNodes); // Get max values FIND_MAX_VALUES // Quantization INIT_QUANTIZATION // Quantize EXWORD Data; for(udword i=0;iHasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data); if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data); } }; Local::_Walk(mNodes, callback, user_data); return true; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_OptimizedTree.h000066400000000000000000000232771207742442300241720ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for optimized trees. * \file OPC_OptimizedTree.h * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_OPTIMIZEDTREE_H__ #define __OPC_OPTIMIZEDTREE_H__ #ifdef __x86_64 #define EXWORD uqword #else #define EXWORD udword #endif //! Common interface for a node of an implicit tree #define IMPLEMENT_IMPLICIT_NODE(base_class, volume) \ public: \ /* Constructor / Destructor */ \ inline_ base_class() : mData(0) {} \ inline_ ~base_class() {} \ /* Leaf test */ \ inline_ BOOL IsLeaf() const { return mData&1; } \ /* Data access */ \ inline_ const base_class* GetPos() const { return (base_class*)mData; } \ inline_ const base_class* GetNeg() const { return ((base_class*)mData)+1; } \ /* Modified by S-cubed, Inc. */ \ inline_ const base_class* GetB() const { return mB; } \ inline_ udword GetPrimitive() const { return (mData>>1); } \ /* Stats */ \ inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \ \ volume mAABB; \ EXWORD mData; \ /* Modified by S-cubed, Inc. */ \ base_class* mB; //! Common interface for a node of a no-leaf tree #define IMPLEMENT_NOLEAF_NODE(base_class, volume) \ public: \ /* Constructor / Destructor */ \ inline_ base_class() : mPosData(0), mNegData(0) {} \ inline_ ~base_class() {} \ /* Leaf tests */ \ inline_ BOOL HasPosLeaf() const { return mPosData&1; } \ inline_ BOOL HasNegLeaf() const { return mNegData&1; } \ /* Data access */ \ inline_ const base_class* GetPos() const { return (base_class*)mPosData; } \ inline_ const base_class* GetNeg() const { return (base_class*)mNegData; } \ /* Modified by S-cubed, Inc. */ \ inline_ const base_class* GetB() const { return mB; } \ inline_ udword GetPosPrimitive() const { return (mPosData>>1); } \ inline_ udword GetNegPrimitive() const { return (mNegData>>1); } \ /* Stats */ \ inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \ \ volume mAABB; \ EXWORD mPosData; \ EXWORD mNegData; \ /* Modified by S-cubed, Inc. */ \ base_class* mB; class OPCODE_API AABBCollisionNode { IMPLEMENT_IMPLICIT_NODE(AABBCollisionNode, CollisionAABB) inline_ float GetVolume() const { return mAABB.mExtents.x * mAABB.mExtents.y * mAABB.mExtents.z; } inline_ float GetSize() const { return mAABB.mExtents.SquareMagnitude(); } inline_ udword GetRadius() const { udword* Bits = (udword*)&mAABB.mExtents.x; udword Max = Bits[0]; if(Bits[1]>Max) Max = Bits[1]; if(Bits[2]>Max) Max = Bits[2]; return Max; } // NB: using the square-magnitude or the true volume of the box, seems to yield better results // (assuming UNC-like informed traversal methods). I borrowed this idea from PQP. The usual "size" // otherwise, is the largest box extent. In SOLID that extent is computed on-the-fly each time it's // needed (the best approach IMHO). In RAPID the rotation matrix is permuted so that Extent[0] is // always the greatest, which saves looking for it at runtime. On the other hand, it yields matrices // whose determinant is not 1, i.e. you can't encode them anymore as unit quaternions. Not a very // good strategy. }; class OPCODE_API AABBQuantizedNode { IMPLEMENT_IMPLICIT_NODE(AABBQuantizedNode, QuantizedAABB) inline_ uword GetSize() const { const uword* Bits = mAABB.mExtents; uword Max = Bits[0]; if(Bits[1]>Max) Max = Bits[1]; if(Bits[2]>Max) Max = Bits[2]; return Max; } // NB: for quantized nodes I don't feel like computing a square-magnitude with integers all // over the place.......! }; class OPCODE_API AABBNoLeafNode { IMPLEMENT_NOLEAF_NODE(AABBNoLeafNode, CollisionAABB) }; class OPCODE_API AABBQuantizedNoLeafNode { IMPLEMENT_NOLEAF_NODE(AABBQuantizedNoLeafNode, QuantizedAABB) }; //! Common interface for a collision tree #define IMPLEMENT_COLLISION_TREE(base_class, node) \ public: \ /* Constructor / Destructor */ \ base_class(); \ virtual ~base_class(); \ /* Builds from a standard tree */ \ override(AABBOptimizedTree) bool Build(AABBTree* tree); \ /* Refits the tree */ \ override(AABBOptimizedTree) bool Refit(const MeshInterface* mesh_interface); \ /* Walks the tree */ \ override(AABBOptimizedTree) bool Walk(GenericWalkingCallback callback, void* user_data) const; \ /* Data access */ \ inline_ const node* GetNodes() const { return mNodes; } \ /* Stats */ \ override(AABBOptimizedTree) udword GetUsedBytes() const { return mNbNodes*sizeof(node); } \ private: \ node* mNodes; typedef bool (*GenericWalkingCallback) (const void* current, void* user_data); class OPCODE_API AABBOptimizedTree { public: // Constructor / Destructor AABBOptimizedTree() : mNbNodes (0) {} virtual ~AABBOptimizedTree() {} /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds the collision tree from a generic AABB tree. * \param tree [in] generic AABB tree * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// virtual bool Build(AABBTree* tree) = 0; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the collision tree after vertices have been modified. * \param mesh_interface [in] mesh interface for current model * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// virtual bool Refit(const MeshInterface* mesh_interface) = 0; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Walks the tree and call the user back for each node. * \param callback [in] walking callback * \param user_data [in] callback's user data * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// virtual bool Walk(GenericWalkingCallback callback, void* user_data) const = 0; // Data access virtual udword GetUsedBytes() const = 0; inline_ udword GetNbNodes() const { return mNbNodes; } protected: udword mNbNodes; }; class OPCODE_API AABBCollisionTree : public AABBOptimizedTree { IMPLEMENT_COLLISION_TREE(AABBCollisionTree, AABBCollisionNode) }; class OPCODE_API AABBNoLeafTree : public AABBOptimizedTree { IMPLEMENT_COLLISION_TREE(AABBNoLeafTree, AABBNoLeafNode) }; class OPCODE_API AABBQuantizedTree : public AABBOptimizedTree { IMPLEMENT_COLLISION_TREE(AABBQuantizedTree, AABBQuantizedNode) public: Point mCenterCoeff; Point mExtentsCoeff; }; class OPCODE_API AABBQuantizedNoLeafTree : public AABBOptimizedTree { IMPLEMENT_COLLISION_TREE(AABBQuantizedNoLeafTree, AABBQuantizedNoLeafNode) public: Point mCenterCoeff; Point mExtentsCoeff; }; #endif // __OPC_OPTIMIZEDTREE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Picking.cpp000066400000000000000000000122551207742442300233170ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code to perform "picking". * \file OPC_Picking.cpp * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; #ifdef OPC_RAYHIT_CALLBACK /* Possible RayCollider usages: - boolean query (shadow feeler) - closest hit - all hits - number of intersection (boolean) */ bool Opcode::SetupAllHits(RayCollider& collider, CollisionFaces& contacts) { struct Local { static void AllContacts(const CollisionFace& hit, void* user_data) { CollisionFaces* CF = (CollisionFaces*)user_data; CF->AddFace(hit); } }; collider.SetFirstContact(false); collider.SetHitCallback(Local::AllContacts); collider.SetUserData(&contacts); return true; } bool Opcode::SetupClosestHit(RayCollider& collider, CollisionFace& closest_contact) { struct Local { static void ClosestContact(const CollisionFace& hit, void* user_data) { CollisionFace* CF = (CollisionFace*)user_data; if(hit.mDistancemDistance) *CF = hit; } }; collider.SetFirstContact(false); collider.SetHitCallback(Local::ClosestContact); collider.SetUserData(&closest_contact); closest_contact.mDistance = MAX_FLOAT; return true; } bool Opcode::SetupShadowFeeler(RayCollider& collider) { collider.SetFirstContact(true); collider.SetHitCallback(null); return true; } bool Opcode::SetupInOutTest(RayCollider& collider) { collider.SetFirstContact(false); collider.SetHitCallback(null); // Results with collider.GetNbIntersections() return true; } bool Opcode::Picking( CollisionFace& picked_face, const Ray& world_ray, const Model& model, const Matrix4x4* world, float min_dist, float max_dist, const Point& view_point, CullModeCallback callback, void* user_data) { struct Local { struct CullData { CollisionFace* Closest; float MinLimit; CullModeCallback Callback; void* UserData; Point ViewPoint; const MeshInterface* IMesh; }; // Called for each stabbed face static void RenderCullingCallback(const CollisionFace& hit, void* user_data) { CullData* Data = (CullData*)user_data; // Discard face if we already have a closer hit if(hit.mDistance>=Data->Closest->mDistance) return; // Discard face if hit point is smaller than min limit. This mainly happens when the face is in front // of the near clip plane (or straddles it). If we keep the face nonetheless, the user can select an // object that he may not even be able to see, which is very annoying. if(hit.mDistance<=Data->MinLimit) return; // This is the index of currently stabbed triangle. udword StabbedFaceIndex = hit.mFaceID; // We may keep it or not, depending on backface culling bool KeepIt = true; // Catch *render* cull mode for this face CullMode CM = (Data->Callback)(StabbedFaceIndex, Data->UserData); if(CM!=CULLMODE_NONE) // Don't even compute culling for double-sided triangles { // Compute backface culling for current face VertexPointers VP; Data->IMesh->GetTriangle(VP, StabbedFaceIndex); if(VP.BackfaceCulling(Data->ViewPoint)) { if(CM==CULLMODE_CW) KeepIt = false; } else { if(CM==CULLMODE_CCW) KeepIt = false; } } if(KeepIt) *Data->Closest = hit; } }; RayCollider RC; RC.SetMaxDist(max_dist); RC.SetTemporalCoherence(false); RC.SetCulling(false); // We need all faces since some of them can be double-sided RC.SetFirstContact(false); RC.SetHitCallback(Local::RenderCullingCallback); picked_face.mFaceID = INVALID_ID; picked_face.mDistance = MAX_FLOAT; picked_face.mU = 0.0f; picked_face.mV = 0.0f; Local::CullData Data; Data.Closest = &picked_face; Data.MinLimit = min_dist; Data.Callback = callback; Data.UserData = user_data; Data.ViewPoint = view_point; Data.IMesh = model.GetMeshInterface(); if(world) { // Get matrices Matrix4x4 InvWorld; InvertPRMatrix(InvWorld, *world); // Compute camera position in mesh space Data.ViewPoint *= InvWorld; } RC.SetUserData(&Data); if(RC.Collide(world_ray, model, world)) { return picked_face.mFaceID!=INVALID_ID; } return false; } #endif choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Picking.h000066400000000000000000000040541207742442300227620ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code to perform "picking". * \file OPC_Picking.h * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_PICKING_H__ #define __OPC_PICKING_H__ #ifdef OPC_RAYHIT_CALLBACK enum CullMode { CULLMODE_NONE = 0, CULLMODE_CW = 1, CULLMODE_CCW = 2 }; typedef CullMode (*CullModeCallback)(udword triangle_index, void* user_data); OPCODE_API bool SetupAllHits (RayCollider& collider, CollisionFaces& contacts); OPCODE_API bool SetupClosestHit (RayCollider& collider, CollisionFace& closest_contact); OPCODE_API bool SetupShadowFeeler (RayCollider& collider); OPCODE_API bool SetupInOutTest (RayCollider& collider); OPCODE_API bool Picking( CollisionFace& picked_face, const Ray& world_ray, const Model& model, const Matrix4x4* world, float min_dist, float max_dist, const Point& view_point, CullModeCallback callback, void* user_data); #endif #endif //__OPC_PICKING_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_PlanesAABBOverlap.h000066400000000000000000000050001207742442300245470ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Planes-AABB overlap test. * - original code by Ville Miettinen, from Umbra/dPVS (released on the GD-Algorithms mailing list) * - almost used "as-is", I even left the comments (hence the frustum-related notes) * * \param center [in] box center * \param extents [in] box extents * \param out_clip_mask [out] bitmask for active planes * \param in_clip_mask [in] bitmask for active planes * \return TRUE if boxes overlap planes */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL PlanesCollider::PlanesAABBOverlap(const Point& center, const Point& extents, udword& out_clip_mask, udword in_clip_mask) { // Stats mNbVolumeBVTests++; const Plane* p = mPlanes; // Evaluate through all active frustum planes. We determine the relation // between the AABB and a plane by using the concept of "near" and "far" // vertices originally described by Zhang (and later by Moller). Our // variant here uses 3 fabs ops, 6 muls, 7 adds and two floating point // comparisons per plane. The routine early-exits if the AABB is found // to be outside any of the planes. The loop also constructs a new output // clip mask. Most FPUs have a native single-cycle fabsf() operation. udword Mask = 1; // current mask index (1,2,4,8,..) udword TmpOutClipMask = 0; // initialize output clip mask into empty. while(Mask<=in_clip_mask) // keep looping while we have active planes left... { if(in_clip_mask & Mask) // if clip plane is active, process it.. { float NP = extents.x*fabsf(p->n.x) + extents.y*fabsf(p->n.y) + extents.z*fabsf(p->n.z); // ### fabsf could be precomputed float MP = center.x*p->n.x + center.y*p->n.y + center.z*p->n.z + p->d; if(NP < MP) // near vertex behind the clip plane... return FALSE; // .. so there is no intersection.. if((-NP) < MP) // near and far vertices on different sides of plane.. TmpOutClipMask |= Mask; // .. so update the clip mask... } Mask+=Mask; // mk = (1<Distance(*mVP.Vertex[0]); float d1 = p->Distance(*mVP.Vertex[1]); float d2 = p->Distance(*mVP.Vertex[2]); if(d0>0.0f && d1>0.0f && d2>0.0f) return FALSE; // if(!(IR(d0)&SIGN_BITMASK) && !(IR(d1)&SIGN_BITMASK) && !(IR(d2)&SIGN_BITMASK)) return FALSE; } Mask+=Mask; p++; } /* for(udword i=0;i<6;i++) { float d0 = p[i].Distance(mLeafVerts[0]); float d1 = p[i].Distance(mLeafVerts[1]); float d2 = p[i].Distance(mLeafVerts[2]); if(d0>0.0f && d1>0.0f && d2>0.0f) return false; } */ return TRUE; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_RayAABBOverlap.h000066400000000000000000000070751207742442300240760ustar00rootroot00000000000000// Opcode 1.1: ray-AABB overlap tests based on Woo's code // Opcode 1.2: ray-AABB overlap tests based on the separating axis theorem // // The point of intersection is not computed anymore. The distance to impact is not needed anymore // since we now have two different queries for segments or rays. /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes a segment-AABB overlap test using the separating axis theorem. Segment is cached within the class. * \param center [in] AABB center * \param extents [in] AABB extents * \return true on overlap */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL RayCollider::SegmentAABBOverlap(const Point& center, const Point& extents) { // Stats mNbRayBVTests++; float Dx = mData2.x - center.x; if(fabsf(Dx) > extents.x + mFDir.x) return FALSE; float Dy = mData2.y - center.y; if(fabsf(Dy) > extents.y + mFDir.y) return FALSE; float Dz = mData2.z - center.z; if(fabsf(Dz) > extents.z + mFDir.z) return FALSE; float f; f = mData.y * Dz - mData.z * Dy; if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y) return FALSE; f = mData.z * Dx - mData.x * Dz; if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x) return FALSE; f = mData.x * Dy - mData.y * Dx; if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x) return FALSE; return TRUE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes a ray-AABB overlap test using the separating axis theorem. Ray is cached within the class. * \param center [in] AABB center * \param extents [in] AABB extents * \return true on overlap */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL RayCollider::RayAABBOverlap(const Point& center, const Point& extents) { // Stats mNbRayBVTests++; // float Dx = mOrigin.x - center.x; if(fabsf(Dx) > extents.x && Dx*mDir.x>=0.0f) return FALSE; // float Dy = mOrigin.y - center.y; if(fabsf(Dy) > extents.y && Dy*mDir.y>=0.0f) return FALSE; // float Dz = mOrigin.z - center.z; if(fabsf(Dz) > extents.z && Dz*mDir.z>=0.0f) return FALSE; float Dx = mOrigin.x - center.x; if(GREATER(Dx, extents.x) && Dx*mDir.x>=0.0f) return FALSE; float Dy = mOrigin.y - center.y; if(GREATER(Dy, extents.y) && Dy*mDir.y>=0.0f) return FALSE; float Dz = mOrigin.z - center.z; if(GREATER(Dz, extents.z) && Dz*mDir.z>=0.0f) return FALSE; // float Dx = mOrigin.x - center.x; if(GREATER(Dx, extents.x) && ((SIR(Dx)-1)^SIR(mDir.x))>=0.0f) return FALSE; // float Dy = mOrigin.y - center.y; if(GREATER(Dy, extents.y) && ((SIR(Dy)-1)^SIR(mDir.y))>=0.0f) return FALSE; // float Dz = mOrigin.z - center.z; if(GREATER(Dz, extents.z) && ((SIR(Dz)-1)^SIR(mDir.z))>=0.0f) return FALSE; float f; f = mDir.y * Dz - mDir.z * Dy; if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y) return FALSE; f = mDir.z * Dx - mDir.x * Dz; if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x) return FALSE; f = mDir.x * Dy - mDir.y * Dx; if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x) return FALSE; return TRUE; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_RayCollider.cpp000066400000000000000000000724441207742442300241520ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for a ray collider. * \file OPC_RayCollider.cpp * \author Pierre Terdiman * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a ray-vs-tree collider. * This class performs a stabbing query on an AABB tree, i.e. does a ray-mesh collision. * * HIGHER DISTANCE BOUND: * * If P0 and P1 are two 3D points, let's define: * - d = distance between P0 and P1 * - Origin = P0 * - Direction = (P1 - P0) / d = normalized direction vector * - A parameter t such as a point P on the line (P0,P1) is P = Origin + t * Direction * - t = 0 --> P = P0 * - t = d --> P = P1 * * Then we can define a general "ray" as: * * struct Ray * { * Point Origin; * Point Direction; * }; * * But it actually maps three different things: * - a segment, when 0 <= t <= d * - a half-line, when 0 <= t < +infinity, or -infinity < t <= d * - a line, when -infinity < t < +infinity * * In Opcode, we support segment queries, which yield half-line queries by setting d = +infinity. * We don't support line-queries. If you need them, shift the origin along the ray by an appropriate margin. * * In short, the lower bound is always 0, and you can setup the higher bound "d" with RayCollider::SetMaxDist(). * * Query |segment |half-line |line * --------|-------------------|---------------|---------------- * Usages |-shadow feelers |-raytracing |- * |-sweep tests |-in/out tests | * * FIRST CONTACT: * * - You can setup "first contact" mode or "all contacts" mode with RayCollider::SetFirstContact(). * - In "first contact" mode we return as soon as the ray hits one face. If can be useful e.g. for shadow feelers, where * you want to know whether the path to the light is free or not (a boolean answer is enough). * - In "all contacts" mode we return all faces hit by the ray. * * TEMPORAL COHERENCE: * * - You can enable or disable temporal coherence with RayCollider::SetTemporalCoherence(). * - It currently only works in "first contact" mode. * - If temporal coherence is enabled, the previously hit triangle is cached during the first query. Then, next queries * start by colliding the ray against the cached triangle. If they still collide, we return immediately. * * CLOSEST HIT: * * - You can enable or disable "closest hit" with RayCollider::SetClosestHit(). * - It currently only works in "all contacts" mode. * - If closest hit is enabled, faces are sorted by distance on-the-fly and the closest one only is reported. * * BACKFACE CULLING: * * - You can enable or disable backface culling with RayCollider::SetCulling(). * - If culling is enabled, ray will not hit back faces (only front faces). * * * * \class RayCollider * \author Pierre Terdiman * \version 1.3 * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * This class describes a face hit by a ray or segment. * This is a particular class dedicated to stabbing queries. * * \class CollisionFace * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * This class is a dedicated collection of CollisionFace. * * \class CollisionFaces * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; #include "OPC_RayAABBOverlap.h" #include "OPC_RayTriOverlap.h" #define SET_CONTACT(prim_index, flag) \ mNbIntersections++; \ /* Set contact status */ \ mFlags |= flag; \ /* In any case the contact has been found and recorded in mStabbedFace */ \ mStabbedFace.mFaceID = prim_index; #ifdef OPC_RAYHIT_CALLBACK #define HANDLE_CONTACT(prim_index, flag) \ SET_CONTACT(prim_index, flag) \ \ if(mHitCallback) (mHitCallback)(mStabbedFace, mUserData); #define UPDATE_CACHE \ if(cache && GetContactStatus()) \ { \ *cache = mStabbedFace.mFaceID; \ } #else #define HANDLE_CONTACT(prim_index, flag) \ SET_CONTACT(prim_index, flag) \ \ /* Now we can also record it in mStabbedFaces if available */ \ if(mStabbedFaces) \ { \ /* If we want all faces or if that's the first one we hit */ \ if(!mClosestHit || !mStabbedFaces->GetNbFaces()) \ { \ mStabbedFaces->AddFace(mStabbedFace); \ } \ else \ { \ /* We only keep closest hit */ \ CollisionFace* Current = const_cast(mStabbedFaces->GetFaces()); \ if(Current && mStabbedFace.mDistancemDistance) \ { \ *Current = mStabbedFace; \ } \ } \ } #define UPDATE_CACHE \ if(cache && GetContactStatus() && mStabbedFaces) \ { \ const CollisionFace* Current = mStabbedFaces->GetFaces(); \ if(Current) *cache = Current->mFaceID; \ else *cache = INVALID_ID; \ } #endif #define SEGMENT_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ \ /* Perform ray-tri overlap test and return */ \ if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ { \ /* Intersection point is valid if dist < segment's length */ \ /* We know dist>0 so we can use integers */ \ if(IR(mStabbedFace.mDistance)GetTriangle(VP, prim_index); \ \ /* Perform ray-tri overlap test and return */ \ if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ { \ HANDLE_CONTACT(prim_index, flag) \ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// RayCollider::RayCollider() : mNbRayBVTests (0), mNbRayPrimTests (0), mNbIntersections (0), mCulling (true), #ifdef OPC_RAYHIT_CALLBACK mHitCallback (null), mUserData (0), #else mClosestHit (false), mStabbedFaces (null), #endif mMaxDist (MAX_FLOAT) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// RayCollider::~RayCollider() { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Validates current settings. You should call this method after all the settings and callbacks have been defined. * \return null if everything is ok, else a string describing the problem */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// const char* RayCollider::ValidateSettings() { if(mMaxDist<0.0f) return "Higher distance bound must be positive!"; if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; #ifndef OPC_RAYHIT_CALLBACK if(mClosestHit && FirstContactEnabled()) return "Closest hit doesn't work with ""First contact"" mode!"; if(TemporalCoherenceEnabled() && mClosestHit) return "Temporal coherence can't guarantee to report closest hit!"; #endif if(SkipPrimitiveTests()) return "SkipPrimitiveTests not possible for RayCollider ! (not implemented)"; return null; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Generic stabbing query for generic OPCODE models. After the call, access the results: * - with GetContactStatus() * - in the user-provided destination array * * \param world_ray [in] stabbing ray in world space * \param model [in] Opcode model to collide with * \param world [in] model's world matrix, or null * \param cache [in] a possibly cached face index, or null * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool RayCollider::Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world, udword* cache) { // Checkings if(!Setup(&model)) return false; // Init collision query if(InitQuery(world_ray, world, cache)) return true; if(!model.HasLeafNodes()) { if(model.IsQuantized()) { const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); // Setup dequantization coeffs mCenterCoeff = Tree->mCenterCoeff; mExtentsCoeff = Tree->mExtentsCoeff; // Perform stabbing query if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); else _RayStab(Tree->GetNodes()); } else { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform stabbing query if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); else _RayStab(Tree->GetNodes()); } } else { if(model.IsQuantized()) { const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); // Setup dequantization coeffs mCenterCoeff = Tree->mCenterCoeff; mExtentsCoeff = Tree->mExtentsCoeff; // Perform stabbing query if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); else _RayStab(Tree->GetNodes()); } else { const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); // Perform stabbing query if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); else _RayStab(Tree->GetNodes()); } } // Update cache if needed UPDATE_CACHE return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Initializes a stabbing query : * - reset stats & contact status * - compute ray in local space * - check temporal coherence * * \param world_ray [in] stabbing ray in world space * \param world [in] object's world matrix, or null * \param face_id [in] index of previously stabbed triangle * \return TRUE if we can return immediately * \warning SCALE NOT SUPPORTED. The matrix must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// BOOL RayCollider::InitQuery(const Ray& world_ray, const Matrix4x4* world, udword* face_id) { // Reset stats & contact status Collider::InitQuery(); mNbRayBVTests = 0; mNbRayPrimTests = 0; mNbIntersections = 0; #ifndef OPC_RAYHIT_CALLBACK if(mStabbedFaces) mStabbedFaces->Reset(); #endif // Compute ray in local space // The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests) if(world) { Matrix3x3 InvWorld = *world; mDir = InvWorld * world_ray.mDir; Matrix4x4 World; InvertPRMatrix(World, *world); mOrigin = world_ray.mOrig * World; } else { mDir = world_ray.mDir; mOrigin = world_ray.mOrig; } // 4) Special case: 1-triangle meshes [Opcode 1.3] if(mCurrentModel && mCurrentModel->HasSingleNode()) { // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. if(!SkipPrimitiveTests()) { // Perform overlap test between the unique triangle and the ray (and set contact status if needed) SEGMENT_PRIM(udword(0), OPC_CONTACT) // Return immediately regardless of status return TRUE; } } // Check temporal coherence : // Test previously colliding primitives first if(TemporalCoherenceEnabled() && FirstContactEnabled() && face_id && *face_id!=INVALID_ID) { #ifdef OLD_CODE #ifndef OPC_RAYHIT_CALLBACK if(!mClosestHit) #endif { // Request vertices from the app VertexPointers VP; mIMesh->GetTriangle(VP, *face_id); // Perform ray-cached tri overlap test if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) { // Intersection point is valid if: // - distance is positive (else it can just be a face behind the orig point) // - distance is smaller than a given max distance (useful for shadow feelers) // if(mStabbedFace.mDistance>0.0f && mStabbedFace.mDistanceAddFace(mStabbedFace); #endif return TRUE; } } } #else // New code // We handle both Segment/ray queries with the same segment code, and a possible infinite limit SEGMENT_PRIM(*face_id, OPC_TEMPORAL_CONTACT) // Return immediately if possible if(GetContactStatus()) return TRUE; #endif } // Precompute data (moved after temporal coherence since only needed for ray-AABB) if(IR(mMaxDist)!=IEEE_MAX_FLOAT) { // For Segment-AABB overlap mData = 0.5f * mDir * mMaxDist; mData2 = mOrigin + mData; // Precompute mFDir; mFDir.x = fabsf(mData.x); mFDir.y = fabsf(mData.y); mFDir.z = fabsf(mData.z); } else { // For Ray-AABB overlap // udword x = SIR(mDir.x)-1; // udword y = SIR(mDir.y)-1; // udword z = SIR(mDir.z)-1; // mData.x = FR(x); // mData.y = FR(y); // mData.z = FR(z); // Precompute mFDir; mFDir.x = fabsf(mDir.x); mFDir.y = fabsf(mDir.y); mFDir.z = fabsf(mDir.z); } return FALSE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stabbing query for vanilla AABB trees. * \param world_ray [in] stabbing ray in world space * \param tree [in] AABB tree * \param box_indices [out] indices of stabbed boxes * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool RayCollider::Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices) { // ### bad design here // This is typically called for a scene tree, full of -AABBs-, not full of triangles. // So we don't really have "primitives" to deal with. Hence it doesn't work with // "FirstContact" + "TemporalCoherence". ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) ); // Checkings if(!tree) return false; // Init collision query // Basically this is only called to initialize precomputed data if(InitQuery(world_ray)) return true; // Perform stabbing query if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(tree, box_indices); else _RayStab(tree, box_indices); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for normal AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_SegmentStab(const AABBCollisionNode* node) { // Perform Segment-AABB overlap test if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; if(node->IsLeaf()) { SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT) } else { _SegmentStab(node->GetPos()); if(ContactFound()) return; _SegmentStab(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for quantized AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_SegmentStab(const AABBQuantizedNode* node) { // Dequantize box const QuantizedAABB& Box = node->mAABB; const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); // Perform Segment-AABB overlap test if(!SegmentAABBOverlap(Center, Extents)) return; if(node->IsLeaf()) { SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT) } else { _SegmentStab(node->GetPos()); if(ContactFound()) return; _SegmentStab(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for no-leaf AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_SegmentStab(const AABBNoLeafNode* node) { // Perform Segment-AABB overlap test if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; if(node->HasPosLeaf()) { SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } else _SegmentStab(node->GetPos()); if(ContactFound()) return; if(node->HasNegLeaf()) { SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } else _SegmentStab(node->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for quantized no-leaf AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_SegmentStab(const AABBQuantizedNoLeafNode* node) { // Dequantize box const QuantizedAABB& Box = node->mAABB; const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); // Perform Segment-AABB overlap test if(!SegmentAABBOverlap(Center, Extents)) return; if(node->HasPosLeaf()) { SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } else _SegmentStab(node->GetPos()); if(ContactFound()) return; if(node->HasNegLeaf()) { SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } else _SegmentStab(node->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for vanilla AABB trees. * \param node [in] current collision node * \param box_indices [out] indices of stabbed boxes */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_SegmentStab(const AABBTreeNode* node, Container& box_indices) { // Test the box against the segment Point Center, Extents; node->GetAABB()->GetCenter(Center); node->GetAABB()->GetExtents(Extents); if(!SegmentAABBOverlap(Center, Extents)) return; if(node->IsLeaf()) { box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives()); } else { _SegmentStab(node->GetPos(), box_indices); _SegmentStab(node->GetNeg(), box_indices); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for normal AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_RayStab(const AABBCollisionNode* node) { // Perform Ray-AABB overlap test if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; if(node->IsLeaf()) { RAY_PRIM(node->GetPrimitive(), OPC_CONTACT) } else { _RayStab(node->GetPos()); if(ContactFound()) return; _RayStab(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for quantized AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_RayStab(const AABBQuantizedNode* node) { // Dequantize box const QuantizedAABB& Box = node->mAABB; const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); // Perform Ray-AABB overlap test if(!RayAABBOverlap(Center, Extents)) return; if(node->IsLeaf()) { RAY_PRIM(node->GetPrimitive(), OPC_CONTACT) } else { _RayStab(node->GetPos()); if(ContactFound()) return; _RayStab(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for no-leaf AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_RayStab(const AABBNoLeafNode* node) { // Perform Ray-AABB overlap test if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; if(node->HasPosLeaf()) { RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } else _RayStab(node->GetPos()); if(ContactFound()) return; if(node->HasNegLeaf()) { RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } else _RayStab(node->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for quantized no-leaf AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_RayStab(const AABBQuantizedNoLeafNode* node) { // Dequantize box const QuantizedAABB& Box = node->mAABB; const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); // Perform Ray-AABB overlap test if(!RayAABBOverlap(Center, Extents)) return; if(node->HasPosLeaf()) { RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } else _RayStab(node->GetPos()); if(ContactFound()) return; if(node->HasNegLeaf()) { RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } else _RayStab(node->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive stabbing query for vanilla AABB trees. * \param node [in] current collision node * \param box_indices [out] indices of stabbed boxes */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void RayCollider::_RayStab(const AABBTreeNode* node, Container& box_indices) { // Test the box against the ray Point Center, Extents; node->GetAABB()->GetCenter(Center); node->GetAABB()->GetExtents(Extents); if(!RayAABBOverlap(Center, Extents)) return; if(node->IsLeaf()) { mFlags |= OPC_CONTACT; box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives()); } else { _RayStab(node->GetPos(), box_indices); _RayStab(node->GetNeg(), box_indices); } } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_RayCollider.h000066400000000000000000000304151207742442300236070ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for a ray collider. * \file OPC_RayCollider.h * \author Pierre Terdiman * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_RAYCOLLIDER_H__ #define __OPC_RAYCOLLIDER_H__ class OPCODE_API CollisionFace { public: //! Constructor inline_ CollisionFace() {} //! Destructor inline_ ~CollisionFace() {} udword mFaceID; //!< Index of touched face float mDistance; //!< Distance from collider to hitpoint float mU, mV; //!< Impact barycentric coordinates }; class OPCODE_API CollisionFaces : private Container { public: //! Constructor CollisionFaces() {} //! Destructor ~CollisionFaces() {} inline_ udword GetNbFaces() const { return GetNbEntries()>>2; } inline_ const CollisionFace* GetFaces() const { return (const CollisionFace*)GetEntries(); } inline_ void Reset() { Container::Reset(); } inline_ void AddFace(const CollisionFace& face) { Add(face.mFaceID).Add(face.mDistance).Add(face.mU).Add(face.mV); } }; #ifdef OPC_RAYHIT_CALLBACK /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * User-callback, called by OPCODE to record a hit. * \param hit [in] current hit * \param user_data [in] user-defined data from SetCallback() */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// typedef void (*HitCallback) (const CollisionFace& hit, void* user_data); #endif class OPCODE_API RayCollider : public Collider { public: // Constructor / Destructor RayCollider(); virtual ~RayCollider(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Generic stabbing query for generic OPCODE models. After the call, access the results: * - with GetContactStatus() * - in the user-provided destination array * * \param world_ray [in] stabbing ray in world space * \param model [in] Opcode model to collide with * \param world [in] model's world matrix, or null * \param cache [in] a possibly cached face index, or null * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world=null, udword* cache=null); // bool Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices); // Settings #ifndef OPC_RAYHIT_CALLBACK /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Settings: enable or disable "closest hit" mode. * \param flag [in] true to report closest hit only * \see SetCulling(bool flag) * \see SetMaxDist(float max_dist) * \see SetDestination(StabbedFaces* sf) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetClosestHit(bool flag) { mClosestHit = flag; } #endif /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Settings: enable or disable backface culling. * \param flag [in] true to enable backface culling * \see SetClosestHit(bool flag) * \see SetMaxDist(float max_dist) * \see SetDestination(StabbedFaces* sf) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetCulling(bool flag) { mCulling = flag; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Settings: sets the higher distance bound. * \param max_dist [in] higher distance bound. Default = maximal value, for ray queries (else segment) * \see SetClosestHit(bool flag) * \see SetCulling(bool flag) * \see SetDestination(StabbedFaces* sf) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetMaxDist(float max_dist=MAX_FLOAT) { mMaxDist = max_dist; } #ifdef OPC_RAYHIT_CALLBACK inline_ void SetHitCallback(HitCallback cb) { mHitCallback = cb; } inline_ void SetUserData(void* user_data) { mUserData = user_data; } #else /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Settings: sets the destination array for stabbed faces. * \param cf [in] destination array, filled during queries * \see SetClosestHit(bool flag) * \see SetCulling(bool flag) * \see SetMaxDist(float max_dist) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetDestination(CollisionFaces* cf) { mStabbedFaces = cf; } #endif // Stats /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stats: gets the number of Ray-BV overlap tests after a collision query. * \see GetNbRayPrimTests() * \see GetNbIntersections() * \return the number of Ray-BV tests performed during last query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbRayBVTests() const { return mNbRayBVTests; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stats: gets the number of Ray-Triangle overlap tests after a collision query. * \see GetNbRayBVTests() * \see GetNbIntersections() * \return the number of Ray-Triangle tests performed during last query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbRayPrimTests() const { return mNbRayPrimTests; } // In-out test /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stats: gets the number of intersection found after a collision query. Can be used for in/out tests. * \see GetNbRayBVTests() * \see GetNbRayPrimTests() * \return the number of valid intersections during last query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbIntersections() const { return mNbIntersections; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider. * \return null if everything is ok, else a string describing the problem */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(Collider) const char* ValidateSettings(); protected: // Ray in local space Point mOrigin; //!< Ray origin Point mDir; //!< Ray direction (normalized) Point mFDir; //!< fabsf(mDir) Point mData, mData2; // Stabbed faces CollisionFace mStabbedFace; //!< Current stabbed face #ifdef OPC_RAYHIT_CALLBACK HitCallback mHitCallback; //!< Callback used to record a hit void* mUserData; //!< User-defined data #else CollisionFaces* mStabbedFaces; //!< List of stabbed faces #endif // Stats udword mNbRayBVTests; //!< Number of Ray-BV tests udword mNbRayPrimTests; //!< Number of Ray-Primitive tests // In-out test udword mNbIntersections; //!< Number of valid intersections // Dequantization coeffs Point mCenterCoeff; Point mExtentsCoeff; // Settings float mMaxDist; //!< Valid segment on the ray #ifndef OPC_RAYHIT_CALLBACK bool mClosestHit; //!< Report closest hit only #endif bool mCulling; //!< Stab culled faces or not // Internal methods void _SegmentStab(const AABBCollisionNode* node); void _SegmentStab(const AABBNoLeafNode* node); void _SegmentStab(const AABBQuantizedNode* node); void _SegmentStab(const AABBQuantizedNoLeafNode* node); void _SegmentStab(const AABBTreeNode* node, Container& box_indices); void _RayStab(const AABBCollisionNode* node); void _RayStab(const AABBNoLeafNode* node); void _RayStab(const AABBQuantizedNode* node); void _RayStab(const AABBQuantizedNoLeafNode* node); void _RayStab(const AABBTreeNode* node, Container& box_indices); // Overlap tests inline_ BOOL RayAABBOverlap(const Point& center, const Point& extents); inline_ BOOL SegmentAABBOverlap(const Point& center, const Point& extents); inline_ BOOL RayTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2); // Init methods BOOL InitQuery(const Ray& world_ray, const Matrix4x4* world=null, udword* face_id=null); }; #endif // __OPC_RAYCOLLIDER_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_RayTriOverlap.h000066400000000000000000000067211207742442300241440ustar00rootroot00000000000000#define LOCAL_EPSILON 0.000001f /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes a ray-triangle intersection test. * Original code from Tomas Möller's "Fast Minimum Storage Ray-Triangle Intersection". * It's been optimized a bit with integer code, and modified to return a non-intersection if distance from * ray origin to triangle is negative. * * \param vert0 [in] triangle vertex * \param vert1 [in] triangle vertex * \param vert2 [in] triangle vertex * \return true on overlap. mStabbedFace is filled with relevant info. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL RayCollider::RayTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2) { // Stats mNbRayPrimTests++; // Find vectors for two edges sharing vert0 Point edge1 = vert1 - vert0; Point edge2 = vert2 - vert0; // Begin calculating determinant - also used to calculate U parameter Point pvec = mDir^edge2; // If determinant is near zero, ray lies in plane of triangle float det = edge1|pvec; if(mCulling) { if(det 0. So we can use integer cmp. // Calculate distance from vert0 to ray origin Point tvec = mOrigin - vert0; // Calculate U parameter and test bounds mStabbedFace.mU = tvec|pvec; // if(IR(u)&0x80000000 || u>det) return FALSE; if(IS_NEGATIVE_FLOAT(mStabbedFace.mU) || IR(mStabbedFace.mU)>IR(det)) return FALSE; // Prepare to test V parameter Point qvec = tvec^edge1; // Calculate V parameter and test bounds mStabbedFace.mV = mDir|qvec; if(IS_NEGATIVE_FLOAT(mStabbedFace.mV) || mStabbedFace.mU+mStabbedFace.mV>det) return FALSE; // Calculate t, scale parameters, ray intersects triangle mStabbedFace.mDistance = edge2|qvec; // Det > 0 so we can early exit here // Intersection point is valid if distance is positive (else it can just be a face behind the orig point) if(IS_NEGATIVE_FLOAT(mStabbedFace.mDistance)) return FALSE; // Else go on float OneOverDet = 1.0f / det; mStabbedFace.mDistance *= OneOverDet; mStabbedFace.mU *= OneOverDet; mStabbedFace.mV *= OneOverDet; } else { // the non-culling branch if(det>-LOCAL_EPSILON && det1.0f) return FALSE; if(IS_NEGATIVE_FLOAT(mStabbedFace.mU) || IR(mStabbedFace.mU)>IEEE_1_0) return FALSE; // prepare to test V parameter Point qvec = tvec^edge1; // Calculate V parameter and test bounds mStabbedFace.mV = (mDir|qvec) * OneOverDet; if(IS_NEGATIVE_FLOAT(mStabbedFace.mV) || mStabbedFace.mU+mStabbedFace.mV>1.0f) return FALSE; // Calculate t, ray intersects triangle mStabbedFace.mDistance = (edge2|qvec) * OneOverDet; // Intersection point is valid if distance is positive (else it can just be a face behind the orig point) if(IS_NEGATIVE_FLOAT(mStabbedFace.mDistance)) return FALSE; } return TRUE; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_Settings.h000066400000000000000000000043401207742442300231740ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains compilation flags. * \file OPC_Settings.h * \author Pierre Terdiman * \date May, 12, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_SETTINGS_H__ #define __OPC_SETTINGS_H__ //! Use CPU comparisons (comment that line to use standard FPU compares) // #define OPC_CPU_COMPARE //! Use FCOMI / FCMOV on Pentium-Pro based processors (comment that line to use plain C++) #define OPC_USE_FCOMI //! Use epsilon value in tri-tri overlap test #define OPC_TRITRI_EPSILON_TEST //! Use tree-coherence or not [not implemented yet] // #define OPC_USE_TREE_COHERENCE //! Use callbacks or direct pointers. Using callbacks might be a bit slower (but probably not much) // #define OPC_USE_CALLBACKS //! Support triangle and vertex strides or not. Using strides might be a bit slower (but probably not much) // #define OPC_USE_STRIDE //! Discard negative pointer in vanilla trees #define OPC_NO_NEG_VANILLA_TREE //! Use a callback in the ray collider #define OPC_RAYHIT_CALLBACK // NB: no compilation flag to enable/disable stats since they're actually needed in the box/box overlap test #endif //__OPC_SETTINGS_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_SphereAABBOverlap.h000066400000000000000000000043641207742442300245670ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Sphere-AABB overlap test, based on Jim Arvo's code. * \param center [in] box center * \param extents [in] box extents * \return TRUE on overlap */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL SphereCollider::SphereAABBOverlap(const Point& center, const Point& extents) { // Stats mNbVolumeBVTests++; float d = 0.0f; //find the square of the distance //from the sphere to the box #ifdef OLDIES for(udword i=0;i<3;i++) { float tmp = mCenter[i] - center[i]; float s = tmp + extents[i]; if(s<0.0f) d += s*s; else { s = tmp - extents[i]; if(s>0.0f) d += s*s; } } #endif //#ifdef NEW_TEST // float tmp = mCenter.x - center.x; // float s = tmp + extents.x; float tmp,s; tmp = mCenter.x - center.x; s = tmp + extents.x; if(s<0.0f) { d += s*s; if(d>mRadius2) return FALSE; } else { s = tmp - extents.x; if(s>0.0f) { d += s*s; if(d>mRadius2) return FALSE; } } tmp = mCenter.y - center.y; s = tmp + extents.y; if(s<0.0f) { d += s*s; if(d>mRadius2) return FALSE; } else { s = tmp - extents.y; if(s>0.0f) { d += s*s; if(d>mRadius2) return FALSE; } } tmp = mCenter.z - center.z; s = tmp + extents.z; if(s<0.0f) { d += s*s; if(d>mRadius2) return FALSE; } else { s = tmp - extents.z; if(s>0.0f) { d += s*s; if(d>mRadius2) return FALSE; } } //#endif #ifdef OLDIES // Point Min = center - extents; // Point Max = center + extents; float d = 0.0f; //find the square of the distance //from the sphere to the box for(udword i=0;i<3;i++) { float Min = center[i] - extents[i]; // if(mCenter[i]Max[i]) if(mCenter[i]>Max) { float s = mCenter[i] - Max; d += s*s; } } } #endif return d <= mRadius2; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_SphereCollider.cpp000066400000000000000000000704161207742442300246420ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for a sphere collider. * \file OPC_SphereCollider.cpp * \author Pierre Terdiman * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains a sphere-vs-tree collider. * This class performs a collision test between a sphere and an AABB tree. You can use this to do a standard player vs world collision, * in a Nettle/Telemachos way. It doesn't suffer from all reported bugs in those two classic codes - the "new" one by Paul Nettle is a * debuggued version I think. Collision response can be driven by reported collision data - it works extremely well for me. In sake of * efficiency, all meshes (that is, all AABB trees) should of course also be kept in an extra hierarchical structure (octree, whatever). * * \class SphereCollider * \author Pierre Terdiman * \version 1.3 * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; #include "OPC_SphereAABBOverlap.h" #include "OPC_SphereTriOverlap.h" #define SET_CONTACT(prim_index, flag) \ /* Set contact status */ \ mFlags |= flag; \ mTouchedPrimitives->Add(prim_index); //! Sphere-triangle overlap test #define SPHERE_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ \ /* Perform sphere-tri overlap test */ \ if(SphereTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ { \ SET_CONTACT(prim_index, flag) \ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SphereCollider::SphereCollider() { mCenter.Zero(); mRadius2 = 0.0f; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SphereCollider::~SphereCollider() { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Generic collision query for generic OPCODE models. After the call, access the results: * - with GetContactStatus() * - with GetNbTouchedPrimitives() * - with GetTouchedPrimitives() * * \param cache [in/out] a sphere cache * \param sphere [in] collision sphere in local space * \param model [in] Opcode model to collide with * \param worlds [in] sphere's world matrix, or null * \param worldm [in] model's world matrix, or null * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm) { // Checkings if(!Setup(&model)) return false; // Init collision query if(InitQuery(cache, sphere, worlds, worldm)) return true; if(!model.HasLeafNodes()) { if(model.IsQuantized()) { const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); // Setup dequantization coeffs mCenterCoeff = Tree->mCenterCoeff; mExtentsCoeff = Tree->mExtentsCoeff; // Perform collision query if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); else _Collide(Tree->GetNodes()); } else { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform collision query if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); else _Collide(Tree->GetNodes()); } } else { if(model.IsQuantized()) { const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); // Setup dequantization coeffs mCenterCoeff = Tree->mCenterCoeff; mExtentsCoeff = Tree->mExtentsCoeff; // Perform collision query if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); else _Collide(Tree->GetNodes()); } else { const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); // Perform collision query if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); else _Collide(Tree->GetNodes()); } } return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Initializes a collision query : * - reset stats & contact status * - setup matrices * - check temporal coherence * * \param cache [in/out] a sphere cache * \param sphere [in] sphere in local space * \param worlds [in] sphere's world matrix, or null * \param worldm [in] model's world matrix, or null * \return TRUE if we can return immediately * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// BOOL SphereCollider::InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds, const Matrix4x4* worldm) { // 1) Call the base method VolumeCollider::InitQuery(); // 2) Compute sphere in model space: // - Precompute R^2 mRadius2 = sphere.mRadius * sphere.mRadius; // - Compute center position mCenter = sphere.mCenter; // -> to world space if(worlds) mCenter *= *worlds; // -> to model space if(worldm) { // Invert model matrix Matrix4x4 InvWorldM; InvertPRMatrix(InvWorldM, *worldm); mCenter *= InvWorldM; } // 3) Setup destination pointer mTouchedPrimitives = &cache.TouchedPrimitives; // 4) Special case: 1-triangle meshes [Opcode 1.3] if(mCurrentModel && mCurrentModel->HasSingleNode()) { if(!SkipPrimitiveTests()) { // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. mTouchedPrimitives->Reset(); // Perform overlap test between the unique triangle and the sphere (and set contact status if needed) SPHERE_PRIM(udword(0), OPC_CONTACT) // Return immediately regardless of status return TRUE; } } // 5) Check temporal coherence : if(TemporalCoherenceEnabled()) { // Here we use temporal coherence // => check results from previous frame before performing the collision query if(FirstContactEnabled()) { // We're only interested in the first contact found => test the unique previously touched face if(mTouchedPrimitives->GetNbEntries()) { // Get index of previously touched face = the first entry in the array udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0); // Then reset the array: // - if the overlap test below is successful, the index we'll get added back anyway // - if it isn't, then the array should be reset anyway for the normal query mTouchedPrimitives->Reset(); // Perform overlap test between the cached triangle and the sphere (and set contact status if needed) SPHERE_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT) // Return immediately if possible if(GetContactStatus()) return TRUE; } // else no face has been touched during previous query // => we'll have to perform a normal query } else { // We're interested in all contacts =>test the new real sphere N(ew) against the previous fat sphere P(revious): float r = sqrtf(cache.FatRadius2) - sphere.mRadius; if(IsCacheValid(cache) && cache.Center.SquareDistance(mCenter) < r*r) { // - if N is included in P, return previous list // => we simply leave the list (mTouchedFaces) unchanged // Set contact status if needed if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT; // In any case we don't need to do a query return TRUE; } else { // - else do the query using a fat N // Reset cache since we'll about to perform a real query mTouchedPrimitives->Reset(); // Make a fat sphere so that coherence will work for subsequent frames mRadius2 *= cache.FatCoeff; // mRadius2 = (sphere.mRadius * cache.FatCoeff)*(sphere.mRadius * cache.FatCoeff); // Update cache with query data (signature for cached faces) cache.Center = mCenter; cache.FatRadius2 = mRadius2; } } } else { // Here we don't use temporal coherence => do a normal query mTouchedPrimitives->Reset(); } return FALSE; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Collision query for vanilla AABB trees. * \param cache [in/out] a sphere cache * \param sphere [in] collision sphere in world space * \param tree [in] AABB tree * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree) { // This is typically called for a scene tree, full of -AABBs-, not full of triangles. // So we don't really have "primitives" to deal with. Hence it doesn't work with // "FirstContact" + "TemporalCoherence". ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) ); // Checkings if(!tree) return false; // Init collision query if(InitQuery(cache, sphere)) return true; // Perform collision query _Collide(tree); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks the sphere completely contains the box. In which case we can end the query sooner. * \param bc [in] box center * \param be [in] box extents * \return true if the sphere contains the whole box */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL SphereCollider::SphereContainsBox(const Point& bc, const Point& be) { // I assume if all 8 box vertices are inside the sphere, so does the whole box. // Sounds ok but maybe there's a better way? Point p; p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z+be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z-be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; return TRUE; } #define TEST_BOX_IN_SPHERE(center, extents) \ if(SphereContainsBox(center, extents)) \ { \ /* Set contact status */ \ mFlags |= OPC_CONTACT; \ _Dump(node); \ return; \ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for normal AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_Collide(const AABBCollisionNode* node) { // Perform Sphere-AABB overlap test if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents) if(node->IsLeaf()) { SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT) } else { _Collide(node->GetPos()); if(ContactFound()) return; _Collide(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for normal AABB trees, without primitive tests. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node) { // Perform Sphere-AABB overlap test if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents) if(node->IsLeaf()) { SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) } else { _CollideNoPrimitiveTest(node->GetPos()); if(ContactFound()) return; _CollideNoPrimitiveTest(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for quantized AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_Collide(const AABBQuantizedNode* node) { // Dequantize box const QuantizedAABB& Box = node->mAABB; const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); // Perform Sphere-AABB overlap test if(!SphereAABBOverlap(Center, Extents)) return; TEST_BOX_IN_SPHERE(Center, Extents) if(node->IsLeaf()) { SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT) } else { _Collide(node->GetPos()); if(ContactFound()) return; _Collide(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for quantized AABB trees, without primitive tests. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node) { // Dequantize box const QuantizedAABB& Box = node->mAABB; const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); // Perform Sphere-AABB overlap test if(!SphereAABBOverlap(Center, Extents)) return; TEST_BOX_IN_SPHERE(Center, Extents) if(node->IsLeaf()) { SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) } else { _CollideNoPrimitiveTest(node->GetPos()); if(ContactFound()) return; _CollideNoPrimitiveTest(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for no-leaf AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_Collide(const AABBNoLeafNode* node) { // Perform Sphere-AABB overlap test if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents) if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } else _Collide(node->GetPos()); if(ContactFound()) return; if(node->HasNegLeaf()) { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } else _Collide(node->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for no-leaf AABB trees, without primitive tests. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node) { // Perform Sphere-AABB overlap test if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents) if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } else _CollideNoPrimitiveTest(node->GetPos()); if(ContactFound()) return; if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } else _CollideNoPrimitiveTest(node->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for quantized no-leaf AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_Collide(const AABBQuantizedNoLeafNode* node) { // Dequantize box const QuantizedAABB& Box = node->mAABB; const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); // Perform Sphere-AABB overlap test if(!SphereAABBOverlap(Center, Extents)) return; TEST_BOX_IN_SPHERE(Center, Extents) if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } else _Collide(node->GetPos()); if(ContactFound()) return; if(node->HasNegLeaf()) { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } else _Collide(node->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for quantized no-leaf AABB trees, without primitive tests. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node) { // Dequantize box const QuantizedAABB& Box = node->mAABB; const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); // Perform Sphere-AABB overlap test if(!SphereAABBOverlap(Center, Extents)) return; TEST_BOX_IN_SPHERE(Center, Extents) if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } else _CollideNoPrimitiveTest(node->GetPos()); if(ContactFound()) return; if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } else _CollideNoPrimitiveTest(node->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for vanilla AABB trees. * \param node [in] current collision node */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void SphereCollider::_Collide(const AABBTreeNode* node) { // Perform Sphere-AABB overlap test Point Center, Extents; node->GetAABB()->GetCenter(Center); node->GetAABB()->GetExtents(Extents); if(!SphereAABBOverlap(Center, Extents)) return; if(node->IsLeaf() || SphereContainsBox(Center, Extents)) { mFlags |= OPC_CONTACT; mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives()); } else { _Collide(node->GetPos()); _Collide(node->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// HybridSphereCollider::HybridSphereCollider() { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// HybridSphereCollider::~HybridSphereCollider() { } bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds, const Matrix4x4* worldm) { // We don't want primitive tests here! mFlags |= OPC_NO_PRIMITIVE_TESTS; // Checkings if(!Setup(&model)) return false; // Init collision query if(InitQuery(cache, sphere, worlds, worldm)) return true; // Special case for 1-leaf trees if(mCurrentModel && mCurrentModel->HasSingleNode()) { // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles udword Nb = mIMesh->GetNbTriangles(); // Loop through all triangles for(udword i=0;imCenterCoeff; mExtentsCoeff = Tree->mExtentsCoeff; // Perform collision query - we don't want primitive tests here! _CollideNoPrimitiveTest(Tree->GetNodes()); } else { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform collision query - we don't want primitive tests here! _CollideNoPrimitiveTest(Tree->GetNodes()); } } else { if(model.IsQuantized()) { const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); // Setup dequantization coeffs mCenterCoeff = Tree->mCenterCoeff; mExtentsCoeff = Tree->mExtentsCoeff; // Perform collision query - we don't want primitive tests here! _CollideNoPrimitiveTest(Tree->GetNodes()); } else { const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); // Perform collision query - we don't want primitive tests here! _CollideNoPrimitiveTest(Tree->GetNodes()); } } // We only have a list of boxes so far if(GetContactStatus()) { // Reset contact status, since it currently only reflects collisions with leaf boxes Collider::InitQuery(); // Change dest container so that we can use built-in overlap tests and get collided primitives cache.TouchedPrimitives.Reset(); mTouchedPrimitives = &cache.TouchedPrimitives; // Read touched leaf boxes udword Nb = mTouchedBoxes.GetNbEntries(); const udword* Touched = mTouchedBoxes.GetEntries(); const LeafTriangles* LT = model.GetLeafTriangles(); const udword* Indices = model.GetIndices(); // Loop through touched leaves while(Nb--) { const LeafTriangles& CurrentLeaf = LT[*Touched++]; // Each leaf box has a set of triangles udword NbTris = CurrentLeaf.GetNbTriangles(); if(Indices) { const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; // Loop through triangles and test each of them while(NbTris--) { udword TriangleIndex = *T++; SPHERE_PRIM(TriangleIndex, OPC_CONTACT) } } else { udword BaseIndex = CurrentLeaf.GetTriangleIndex(); // Loop through triangles and test each of them while(NbTris--) { udword TriangleIndex = BaseIndex++; SPHERE_PRIM(TriangleIndex, OPC_CONTACT) } } } } return true; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_SphereCollider.h000066400000000000000000000114411207742442300243000ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for a sphere collider. * \file OPC_SphereCollider.h * \author Pierre Terdiman * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_SPHERECOLLIDER_H__ #define __OPC_SPHERECOLLIDER_H__ struct OPCODE_API SphereCache : VolumeCache { SphereCache() : Center(0.0f,0.0f,0.0f), FatRadius2(0.0f), FatCoeff(1.1f) {} ~SphereCache() {} // Cached faces signature Point Center; //!< Sphere used when performing the query resulting in cached faces float FatRadius2; //!< Sphere used when performing the query resulting in cached faces // User settings float FatCoeff; //!< mRadius2 multiplier used to create a fat sphere }; class OPCODE_API SphereCollider : public VolumeCollider { public: // Constructor / Destructor SphereCollider(); virtual ~SphereCollider(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Generic collision query for generic OPCODE models. After the call, access the results: * - with GetContactStatus() * - with GetNbTouchedPrimitives() * - with GetTouchedPrimitives() * * \param cache [in/out] a sphere cache * \param sphere [in] collision sphere in local space * \param model [in] Opcode model to collide with * \param worlds [in] sphere's world matrix, or null * \param worldm [in] model's world matrix, or null * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null); // bool Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree); protected: // Sphere in model space Point mCenter; //!< Sphere center float mRadius2; //!< Sphere radius squared // Internal methods void _Collide(const AABBCollisionNode* node); void _Collide(const AABBNoLeafNode* node); void _Collide(const AABBQuantizedNode* node); void _Collide(const AABBQuantizedNoLeafNode* node); void _Collide(const AABBTreeNode* node); void _CollideNoPrimitiveTest(const AABBCollisionNode* node); void _CollideNoPrimitiveTest(const AABBNoLeafNode* node); void _CollideNoPrimitiveTest(const AABBQuantizedNode* node); void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node); // Overlap tests inline_ BOOL SphereContainsBox(const Point& bc, const Point& be); inline_ BOOL SphereAABBOverlap(const Point& center, const Point& extents); BOOL SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2); // Init methods BOOL InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null); }; class OPCODE_API HybridSphereCollider : public SphereCollider { public: // Constructor / Destructor HybridSphereCollider(); virtual ~HybridSphereCollider(); bool Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null); protected: Container mTouchedBoxes; }; #endif // __OPC_SPHERECOLLIDER_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_SphereTriOverlap.h000066400000000000000000000105351207742442300246350ustar00rootroot00000000000000 // This is collision detection. If you do another distance test for collision *response*, // if might be useful to simply *skip* the test below completely, and report a collision. // - if sphere-triangle overlap, result is ok // - if they don't, we'll discard them during collision response with a similar test anyway // Overall this approach should run faster. // Original code by David Eberly in Magic. BOOL SphereCollider::SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2) { // Stats mNbVolumePrimTests++; // Early exit if one of the vertices is inside the sphere Point kDiff = vert2 - mCenter; float fC = kDiff.SquareMagnitude(); if(fC <= mRadius2) return TRUE; kDiff = vert1 - mCenter; fC = kDiff.SquareMagnitude(); if(fC <= mRadius2) return TRUE; kDiff = vert0 - mCenter; fC = kDiff.SquareMagnitude(); if(fC <= mRadius2) return TRUE; // Else do the full distance test Point TriEdge0 = vert1 - vert0; Point TriEdge1 = vert2 - vert0; //Point kDiff = vert0 - mCenter; float fA00 = TriEdge0.SquareMagnitude(); float fA01 = TriEdge0 | TriEdge1; float fA11 = TriEdge1.SquareMagnitude(); float fB0 = kDiff | TriEdge0; float fB1 = kDiff | TriEdge1; //float fC = kDiff.SquareMagnitude(); float fDet = fabsf(fA00*fA11 - fA01*fA01); float u = fA01*fB1-fA11*fB0; float v = fA01*fB0-fA00*fB1; float SqrDist; if(u + v <= fDet) { if(u < 0.0f) { if(v < 0.0f) // region 4 { if(fB0 < 0.0f) { // v = 0.0f; if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; } else { u = -fB0/fA00; SqrDist = fB0*u+fC; } } else { // u = 0.0f; if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; } else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; } else { v = -fB1/fA11; SqrDist = fB1*v+fC; } } } else // region 3 { // u = 0.0f; if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; } else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; } else { v = -fB1/fA11; SqrDist = fB1*v+fC; } } } else if(v < 0.0f) // region 5 { // v = 0.0f; if(fB0>=0.0f) { /*u = 0.0f;*/ SqrDist = fC; } else if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; } else { u = -fB0/fA00; SqrDist = fB0*u+fC; } } else // region 0 { // minimum at interior point if(fDet==0.0f) { // u = 0.0f; // v = 0.0f; SqrDist = MAX_FLOAT; } else { float fInvDet = 1.0f/fDet; u *= fInvDet; v *= fInvDet; SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC; } } } else { float fTmp0, fTmp1, fNumer, fDenom; if(u < 0.0f) // region 2 { fTmp0 = fA01 + fB0; fTmp1 = fA11 + fB1; if(fTmp1 > fTmp0) { fNumer = fTmp1 - fTmp0; fDenom = fA00-2.0f*fA01+fA11; if(fNumer >= fDenom) { // u = 1.0f; // v = 0.0f; SqrDist = fA00+2.0f*fB0+fC; } else { u = fNumer/fDenom; v = 1.0f - u; SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC; } } else { // u = 0.0f; if(fTmp1 <= 0.0f) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; } else if(fB1 >= 0.0f) { /*v = 0.0f;*/ SqrDist = fC; } else { v = -fB1/fA11; SqrDist = fB1*v+fC; } } } else if(v < 0.0f) // region 6 { fTmp0 = fA01 + fB1; fTmp1 = fA00 + fB0; if(fTmp1 > fTmp0) { fNumer = fTmp1 - fTmp0; fDenom = fA00-2.0f*fA01+fA11; if(fNumer >= fDenom) { // v = 1.0f; // u = 0.0f; SqrDist = fA11+2.0f*fB1+fC; } else { v = fNumer/fDenom; u = 1.0f - v; SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC; } } else { // v = 0.0f; if(fTmp1 <= 0.0f) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; } else if(fB0 >= 0.0f) { /*u = 0.0f;*/ SqrDist = fC; } else { u = -fB0/fA00; SqrDist = fB0*u+fC; } } } else // region 1 { fNumer = fA11 + fB1 - fA01 - fB0; if(fNumer <= 0.0f) { // u = 0.0f; // v = 1.0f; SqrDist = fA11+2.0f*fB1+fC; } else { fDenom = fA00-2.0f*fA01+fA11; if(fNumer >= fDenom) { // u = 1.0f; // v = 0.0f; SqrDist = fA00+2.0f*fB0+fC; } else { u = fNumer/fDenom; v = 1.0f - u; SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC; } } } } return fabsf(SqrDist) < mRadius2; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_SweepAndPrune.cpp000066400000000000000000000415771207742442300244640ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide) * \file OPC_SweepAndPrune.cpp * \author Pierre Terdiman * \date January, 29, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; inline_ void Sort(udword& id0, udword& id1) { if(id0>id1) Swap(id0, id1); } class Opcode::SAP_Element { public: inline_ SAP_Element() {} inline_ SAP_Element(udword id, SAP_Element* next) : mID(id), mNext(next) {} inline_ ~SAP_Element() {} udword mID; SAP_Element* mNext; }; class Opcode::SAP_Box { public: SAP_EndPoint* Min[3]; SAP_EndPoint* Max[3]; }; class Opcode::SAP_EndPoint { public: float Value; // Min or Max value SAP_EndPoint* Previous; // Previous EndPoint whose Value is smaller than ours (or null) SAP_EndPoint* Next; // Next EndPoint whose Value is greater than ours (or null) udword Data; // Parent box ID *2 | MinMax flag inline_ void SetData(udword box_id, BOOL is_max) { Data = (box_id<<1)|is_max; } inline_ BOOL IsMax() const { return Data & 1; } inline_ udword GetBoxID() const { return Data>>1; } inline_ void InsertAfter(SAP_EndPoint* element) { if(this!=element && this!=element->Next) { // Remove if(Previous) Previous->Next = Next; if(Next) Next->Previous = Previous; // Insert Next = element->Next; if(Next) Next->Previous = this; element->Next = this; Previous = element; } } inline_ void InsertBefore(SAP_EndPoint* element) { if(this!=element && this!=element->Previous) { // Remove if(Previous) Previous->Next = Next; if(Next) Next->Previous = Previous; // Insert Previous = element->Previous; element->Previous = this; Next = element; if(Previous) Previous->Next = this; } } }; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SAP_PairData::SAP_PairData() : mNbElements (0), mNbUsedElements (0), mElementPool (null), mFirstFree (null), mNbObjects (0), mArray (null) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SAP_PairData::~SAP_PairData() { Release(); } void SAP_PairData::Release() { mNbElements = 0; mNbUsedElements = 0; mNbObjects = 0; DELETEARRAY(mElementPool); DELETEARRAY(mArray); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Initializes. * \param nb_objects [in] * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool SAP_PairData::Init(udword nb_objects) { // Make sure everything has been released Release(); if(!nb_objects) return false; mArray = new SAP_Element*[nb_objects]; CHECKALLOC(mArray); ZeroMemory(mArray, nb_objects*sizeof(SAP_Element*)); mNbObjects = nb_objects; return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Remaps a pointer when pool gets resized. * \param element [in/out] remapped element * \param delta [in] offset in bytes */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void Remap(SAP_Element*& element, udword delta) { if(element) element = (SAP_Element*)(udword(element) + delta); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets a free element in the pool. * \param id [in] element id * \param next [in] next element * \param remap [out] possible remapping offset * \return the new element */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SAP_Element* SAP_PairData::GetFreeElem(udword id, SAP_Element* next, udword* remap) { if(remap) *remap = 0; SAP_Element* FreeElem; if(mFirstFree) { // Recycle FreeElem = mFirstFree; mFirstFree = mFirstFree->mNext; // First free = next free (or null) } else { if(mNbUsedElements==mNbElements) { // Resize mNbElements = mNbElements ? (mNbElements<<1) : 2; SAP_Element* NewElems = new SAP_Element[mNbElements]; if(mNbUsedElements) CopyMemory(NewElems, mElementPool, mNbUsedElements*sizeof(SAP_Element)); // Remap everything { udword Delta = udword(NewElems) - udword(mElementPool); for(udword i=0;imID = id; FreeElem->mNext = next; return FreeElem; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Frees an element of the pool. * \param elem [in] element to free/recycle */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SAP_PairData::FreeElem(SAP_Element* elem) { elem->mNext = mFirstFree; // Next free mFirstFree = elem; } // Add a pair to the set. void SAP_PairData::AddPair(udword id1, udword id2) { // Order the ids Sort(id1, id2); ASSERT(id1=mNbObjects) return; // Select the right list from "mArray". SAP_Element* Current = mArray[id1]; if(!Current) { // Empty slot => create new element mArray[id1] = GetFreeElem(id2, null); } else if(Current->mID>id2) { // The list is not empty but all elements are greater than id2 => insert id2 in the front. mArray[id1] = GetFreeElem(id2, mArray[id1]); } else { // Else find the correct location in the sorted list (ascending order) and insert id2 there. while(Current->mNext) { if(Current->mNext->mID > id2) break; Current = Current->mNext; } if(Current->mID==id2) return; // The pair already exists // Current->mNext = GetFreeElem(id2, Current->mNext); udword Delta; SAP_Element* E = GetFreeElem(id2, Current->mNext, &Delta); if(Delta) Remap(Current, Delta); Current->mNext = E; } } // Delete a pair from the set. void SAP_PairData::RemovePair(udword id1, udword id2) { // Order the ids. Sort(id1, id2); // Exit if the pair doesn't exist in the set if(id1>=mNbObjects) return; // Otherwise, select the correct list. SAP_Element* Current = mArray[id1]; // If this list is empty, the pair doesn't exist. if(!Current) return; // Otherwise, if id2 is the first element, delete it. if(Current->mID==id2) { mArray[id1] = Current->mNext; FreeElem(Current); } else { // If id2 is not the first element, start traversing the sorted list. while(Current->mNext) { // If we have moved too far away without hitting id2, then the pair doesn't exist if(Current->mNext->mID > id2) return; // Otherwise, delete id2. if(Current->mNext->mID == id2) { SAP_Element* Temp = Current->mNext; Current->mNext = Temp->mNext; FreeElem(Temp); return; } Current = Current->mNext; } } } void SAP_PairData::DumpPairs(Pairs& pairs) const { // ### Ugly and slow for(udword i=0;imIDmID); Current = Current->mNext; } } } void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const { if(!callback) return; // ### Ugly and slow for(udword i=0;imIDmID, user_data)) return; Current = Current->mNext; } } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SweepAndPrune::SweepAndPrune() { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SweepAndPrune::~SweepAndPrune() { } void SweepAndPrune::GetPairs(Pairs& pairs) const { mPairs.DumpPairs(pairs); } void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const { mPairs.DumpPairs(callback, user_data); } bool SweepAndPrune::Init(udword nb_objects, const AABB** boxes) { // 1) Create sorted lists mNbObjects = nb_objects; mBoxes = new SAP_Box[nb_objects]; // for(udword i=0;iGetMin(Axis); Data[i*2+1] = boxes[i]->GetMax(Axis); } RadixSort RS; const udword* Sorted = RS.Sort(Data, nb_objects*2).GetRanks(); SAP_EndPoint* PreviousEndPoint = null; for(udword i=0;i>1; ASSERT(BoxIndexValue = SortedCoord; // CurrentEndPoint->IsMax = SortedIndex&1; // ### could be implicit ? // CurrentEndPoint->ID = BoxIndex; // ### could be implicit ? CurrentEndPoint->SetData(BoxIndex, SortedIndex&1); // ### could be implicit ? CurrentEndPoint->Previous = PreviousEndPoint; CurrentEndPoint->Next = null; if(PreviousEndPoint) PreviousEndPoint->Next = CurrentEndPoint; if(CurrentEndPoint->IsMax()) mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint; else mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint; PreviousEndPoint = CurrentEndPoint; } } DELETEARRAY(Data); CheckListsIntegrity(); // 2) Quickly find starting pairs mPairs.Init(nb_objects); { Pairs P; CompleteBoxPruning(nb_objects, boxes, P, Axes(AXES_XZY)); for(udword i=0;iid0; udword id1 = PP->id1; if(id0!=id1 && boxes[id0]->Intersect(*boxes[id1])) { mPairs.AddPair(id0, id1); } else ASSERT(0); } } return true; } bool SweepAndPrune::CheckListsIntegrity() { for(udword Axis=0;Axis<3;Axis++) { // Find list head SAP_EndPoint* Current = mList[Axis]; while(Current->Previous) Current = Current->Previous; udword Nb = 0; SAP_EndPoint* Previous = null; while(Current) { Nb++; if(Previous) { ASSERT(Previous->Value <= Current->Value); if(Previous->Value > Current->Value) return false; } ASSERT(Current->Previous==Previous); if(Current->Previous!=Previous) return false; Previous = Current; Current = Current->Next; } ASSERT(Nb==mNbObjects*2); } return true; } inline_ BOOL Intersect(const AABB& a, const SAP_Box& b) { if(b.Max[0]->Value < a.GetMin(0) || a.GetMax(0) < b.Min[0]->Value || b.Max[1]->Value < a.GetMin(1) || a.GetMax(1) < b.Min[1]->Value || b.Max[2]->Value < a.GetMin(2) || a.GetMax(2) < b.Min[2]->Value) return FALSE; return TRUE; } bool SweepAndPrune::UpdateObject(udword i, const AABB& box) { for(udword Axis=0;Axis<3;Axis++) { // udword Base = (udword)&mList[Axis][0]; // Update min { SAP_EndPoint* const CurrentMin = mBoxes[i].Min[Axis]; ASSERT(!CurrentMin->IsMax()); const float Limit = box.GetMin(Axis); if(Limit == CurrentMin->Value) { } else if(Limit < CurrentMin->Value) { CurrentMin->Value = Limit; // Min is moving left: SAP_EndPoint* NewPos = CurrentMin; ASSERT(NewPos); SAP_EndPoint* tmp; while((tmp = NewPos->Previous) && tmp->Value > Limit) { NewPos = tmp; if(NewPos->IsMax()) { // Our min passed a max => start overlap //udword SortedIndex = (udword(CurrentMin) - Base)/sizeof(NS_EndPoint); const udword id0 = CurrentMin->GetBoxID(); const udword id1 = NewPos->GetBoxID(); if(id0!=id1 && Intersect(box, mBoxes[id1])) mPairs.AddPair(id0, id1); } } CurrentMin->InsertBefore(NewPos); } else// if(Limit > CurrentMin->Value) { CurrentMin->Value = Limit; // Min is moving right: SAP_EndPoint* NewPos = CurrentMin; ASSERT(NewPos); SAP_EndPoint* tmp; while((tmp = NewPos->Next) && tmp->Value < Limit) { NewPos = tmp; if(NewPos->IsMax()) { // Our min passed a max => stop overlap const udword id0 = CurrentMin->GetBoxID(); const udword id1 = NewPos->GetBoxID(); if(id0!=id1) mPairs.RemovePair(id0, id1); } } CurrentMin->InsertAfter(NewPos); } } // Update max { SAP_EndPoint* const CurrentMax = mBoxes[i].Max[Axis]; ASSERT(CurrentMax->IsMax()); const float Limit = box.GetMax(Axis); if(Limit == CurrentMax->Value) { } else if(Limit > CurrentMax->Value) { CurrentMax->Value = Limit; // Max is moving right: SAP_EndPoint* NewPos = CurrentMax; ASSERT(NewPos); SAP_EndPoint* tmp; while((tmp = NewPos->Next) && tmp->Value < Limit) { NewPos = tmp; if(!NewPos->IsMax()) { // Our max passed a min => start overlap const udword id0 = CurrentMax->GetBoxID(); const udword id1 = NewPos->GetBoxID(); if(id0!=id1 && Intersect(box, mBoxes[id1])) mPairs.AddPair(id0, id1); } } CurrentMax->InsertAfter(NewPos); } else// if(Limit < CurrentMax->Value) { CurrentMax->Value = Limit; // Max is moving left: SAP_EndPoint* NewPos = CurrentMax; ASSERT(NewPos); SAP_EndPoint* tmp; while((tmp = NewPos->Previous) && tmp->Value > Limit) { NewPos = tmp; if(!NewPos->IsMax()) { // Our max passed a min => stop overlap const udword id0 = CurrentMax->GetBoxID(); const udword id1 = NewPos->GetBoxID(); if(id0!=id1) mPairs.RemovePair(id0, id1); } } CurrentMax->InsertBefore(NewPos); } } } return true; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_SweepAndPrune.h000066400000000000000000000070461207742442300241220ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide) * \file OPC_SweepAndPrune.h * \author Pierre Terdiman * \date January, 29, 2000 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_SWEEPANDPRUNE_H__ #define __OPC_SWEEPANDPRUNE_H__ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * User-callback, called by OPCODE for each colliding pairs. * \param id0 [in] id of colliding object * \param id1 [in] id of colliding object * \param user_data [in] user-defined data * \return TRUE to continue enumeration */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// typedef BOOL (*PairCallback) (udword id0, udword id1, void* user_data); class SAP_Element; class SAP_EndPoint; class SAP_Box; class OPCODE_API SAP_PairData { public: SAP_PairData(); ~SAP_PairData(); bool Init(udword nb_objects); void AddPair(udword id1, udword id2); void RemovePair(udword id1, udword id2); void DumpPairs(Pairs& pairs) const; void DumpPairs(PairCallback callback, void* user_data) const; private: udword mNbElements; //!< Total number of elements in the pool udword mNbUsedElements; //!< Number of used elements SAP_Element* mElementPool; //!< Array of mNbElements elements SAP_Element* mFirstFree; //!< First free element in the pool udword mNbObjects; //!< Max number of objects we can handle SAP_Element** mArray; //!< Pointers to pool // Internal methods SAP_Element* GetFreeElem(udword id, SAP_Element* next, udword* remap=null); inline_ void FreeElem(SAP_Element* elem); void Release(); }; class OPCODE_API SweepAndPrune { public: SweepAndPrune(); ~SweepAndPrune(); bool Init(udword nb_objects, const AABB** boxes); bool UpdateObject(udword i, const AABB& box); void GetPairs(Pairs& pairs) const; void GetPairs(PairCallback callback, void* user_data) const; private: SAP_PairData mPairs; udword mNbObjects; SAP_Box* mBoxes; SAP_EndPoint* mList[3]; // Internal methods bool CheckListsIntegrity(); }; #endif //__OPC_SWEEPANDPRUNE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_TreeBuilders.cpp000066400000000000000000000271761207742442300243340ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for tree builders. * \file OPC_TreeBuilders.cpp * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A builder for AABB-trees of vertices. * * \class AABBTreeOfVerticesBuilder * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A builder for AABB-trees of AABBs. * * \class AABBTreeOfAABBsBuilder * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * A builder for AABB-trees of triangles. * * \class AABBTreeOfTrianglesBuilder * \author Pierre Terdiman * \version 1.3 * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the AABB of a set of primitives. * \param primitives [in] list of indices of primitives * \param nb_prims [in] number of indices * \param global_box [out] global AABB enclosing the set of input primitives * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeOfAABBsBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const { // Checkings if(!primitives || !nb_prims) return false; // Initialize global box global_box = mAABBArray[primitives[0]]; // Loop through boxes for(udword i=1;iGetTriangle(VP, *primitives++); // Update global box Min.Min(*VP.Vertex[0]).Min(*VP.Vertex[1]).Min(*VP.Vertex[2]); Max.Max(*VP.Vertex[0]).Max(*VP.Vertex[1]).Max(*VP.Vertex[2]); } global_box.SetMinMax(Min, Max); return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the splitting value along a given axis for a given primitive. * \param index [in] index of the primitive to split * \param axis [in] axis index (0,1,2) * \return splitting value */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float AABBTreeOfTrianglesBuilder::GetSplittingValue(udword index, udword axis) const { /* // Compute center of triangle Point Center; mTriList[index].Center(mVerts, Center); // Return value return Center[axis];*/ // Compute correct component from center of triangle // return (mVerts[mTriList[index].mVRef[0]][axis] // +mVerts[mTriList[index].mVRef[1]][axis] // +mVerts[mTriList[index].mVRef[2]][axis])*INV3; VertexPointers VP; mIMesh->GetTriangle(VP, index); // Compute correct component from center of triangle return ((*VP.Vertex[0])[axis] +(*VP.Vertex[1])[axis] +(*VP.Vertex[2])[axis])*INV3; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the splitting value along a given axis for a given node. * \param primitives [in] list of indices of primitives * \param nb_prims [in] number of indices * \param global_box [in] global AABB enclosing the set of input primitives * \param axis [in] axis index (0,1,2) * \return splitting value */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float AABBTreeOfTrianglesBuilder::GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const { if(mSettings.mRules&SPLIT_GEOM_CENTER) { // Loop through triangles float SplitValue = 0.0f; VertexPointers VP; for(udword i=0;iGetTriangle(VP, primitives[i]); // Update split value SplitValue += (*VP.Vertex[0])[axis]; SplitValue += (*VP.Vertex[1])[axis]; SplitValue += (*VP.Vertex[2])[axis]; } return SplitValue / float(nb_prims*3); } else return AABBTreeBuilder::GetSplittingValue(primitives, nb_prims, global_box, axis); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Computes the AABB of a set of primitives. * \param primitives [in] list of indices of primitives * \param nb_prims [in] number of indices * \param global_box [out] global AABB enclosing the set of input primitives * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeOfVerticesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const { // Checkings if(!primitives || !nb_prims) return false; // Initialize global box global_box.SetEmpty(); // Loop through vertices for(udword i=0;i using namespace Opcode; #include "OPC_BoxBoxOverlap.h" //#include "OPC_TriBoxOverlap.h" #include "OPC_TriTriOverlap.h" /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBTreeCollider::AABBTreeCollider() : mIMesh0 (null), mIMesh1 (null), mNbBVBVTests (0), mNbPrimPrimTests (0), mNbBVPrimTests (0), mFullBoxBoxTest (true), mFullPrimBoxTest (true), collisionPairInserter(0) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// AABBTreeCollider::~AABBTreeCollider() { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Validates current settings. You should call this method after all the settings and callbacks have been defined. * \return null if everything is ok, else a string describing the problem */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// const char* AABBTreeCollider::ValidateSettings() { if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; return null; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Generic collision query for generic OPCODE models. After the call, access the results with: * - GetContactStatus() * - GetNbPairs() * - GetPairs() * * \param cache [in] collision cache for model pointers and a colliding pair of primitives * \param world0 [in] world matrix for first object * \param world1 [in] world matrix for second object * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1) { // Checkings if(!cache.Model0 || !cache.Model1) return false; if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false; if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false; /* Rules: - perform hull test - when hulls collide, disable hull test - if meshes overlap, reset countdown - if countdown reaches 0, enable hull test */ #ifdef __MESHMERIZER_H__ // Handle hulls if(cache.HullTest) { if(cache.Model0->GetHull() && cache.Model1->GetHull()) { struct Local { static Point* SVCallback(const Point& sv, udword& previndex, udword user_data) { CollisionHull* Hull = (CollisionHull*)user_data; previndex = Hull->ComputeSupportingVertex(sv, previndex); return (Point*)&Hull->GetVerts()[previndex]; } }; bool Collide; if(0) { static GJKEngine GJK; static bool GJKInitDone=false; if(!GJKInitDone) { GJK.Enable(GJK_BACKUP_PROCEDURE); GJK.Enable(GJK_DEGENERATE); GJK.Enable(GJK_HILLCLIMBING); GJKInitDone = true; } GJK.SetCallbackObj0(Local::SVCallback); GJK.SetCallbackObj1(Local::SVCallback); GJK.SetUserData0(udword(cache.Model0->GetHull())); GJK.SetUserData1(udword(cache.Model1->GetHull())); Collide = GJK.Collide(*world0, *world1, &cache.SepVector); } else { static SVEngine SVE; SVE.SetCallbackObj0(Local::SVCallback); SVE.SetCallbackObj1(Local::SVCallback); SVE.SetUserData0(udword(cache.Model0->GetHull())); SVE.SetUserData1(udword(cache.Model1->GetHull())); Collide = SVE.Collide(*world0, *world1, &cache.SepVector); } if(!Collide) { // Reset stats & contact status mFlags &= ~OPC_CONTACT; mNbBVBVTests = 0; mNbPrimPrimTests = 0; mNbBVPrimTests = 0; mPairs.Reset(); return true; } } } // Here, hulls collide cache.HullTest = false; #endif // __MESHMERIZER_H__ // Checkings if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false; // Simple double-dispatch bool Status; if(!cache.Model0->HasLeafNodes()) { if(cache.Model0->IsQuantized()) { const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree(); const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree(); Status = Collide(T0, T1, world0, world1, &cache); } else { const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree(); const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree(); Status = Collide(T0, T1, world0, world1, &cache); } } else { if(cache.Model0->IsQuantized()) { const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree(); const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree(); Status = Collide(T0, T1, world0, world1, &cache); } else { const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree(); const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree(); Status = Collide(T0, T1, world0, world1, &cache); } } #ifdef __MESHMERIZER_H__ if(Status) { // Reset counter as long as overlap occurs if(GetContactStatus()) cache.ResetCountDown(); // Enable hull test again when counter reaches zero cache.CountDown--; if(!cache.CountDown) { cache.ResetCountDown(); cache.HullTest = true; } } #endif return Status; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Initializes a collision query : * - reset stats & contact status * - setup matrices * * \param world0 [in] world matrix for first object * \param world1 [in] world matrix for second object * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1) { // Reset stats & contact status Collider::InitQuery(); mNbBVBVTests = 0; mNbPrimPrimTests = 0; mNbBVPrimTests = 0; mPairs.Reset(); // Setup matrices Matrix4x4 InvWorld0, InvWorld1; if(world0) InvertPRMatrix(InvWorld0, *world0); else InvWorld0.Identity(); if(world1) InvertPRMatrix(InvWorld1, *world1); else InvWorld1.Identity(); Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1; Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0; mR0to1 = World0to1; World0to1.GetTrans(mT0to1); mR1to0 = World1to0; World1to0.GetTrans(mT1to0); // Precompute absolute 1-to-0 rotation matrix for(udword i=0;i<3;i++) { for(udword j=0;j<3;j++) { // Epsilon value prevents floating-point inaccuracies (strategy borrowed from CD) mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]); } } //Modified by S-cubed, Inc. //$B9TNs$r(B Boost $B$N7A<0$K$"$o$;$k!#(B // Prim-Prim $B%F%9%H$r(B TriOverlap.cpp $B$G9T$&$?$a!#(B if(collisionPairInserter){ for(udword i=0; i<3; i++){ for(udword j=0; j<3; j++){ collisionPairInserter->CD_Rot1(i,j) = (double) world0->m[j][i]; collisionPairInserter->CD_Rot2(i,j) = (double) world1->m[j][i]; } } IceMaths::Point t0; IceMaths::Point t1; world0->GetTrans(t0); world1->GetTrans(t1); // Matrix3x3 w1 = world1; // mT0to1 = w1 * (t1 - t0) collisionPairInserter->CD_Trans1[0] = (double) t0.x; collisionPairInserter->CD_Trans1[1] = (double) t0.y; collisionPairInserter->CD_Trans1[2] = (double) t0.z; collisionPairInserter->CD_Trans2[0] = (double) t1.x; collisionPairInserter->CD_Trans2[1] = (double) t1.y; collisionPairInserter->CD_Trans2[2] = (double) t1.z; //s1, s2 $B$O(B 1.0 $B$N$_$J$N$G!"8GDj!#(B collisionPairInserter->CD_s1 = 1.0; collisionPairInserter->CD_s2 = 1.0; } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Takes advantage of temporal coherence. * \param cache [in] cache for a pair of previously colliding primitives * \return true if we can return immediately * \warning only works for "First Contact" mode */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache) { // Checkings if(!cache) return false; // Test previously colliding primitives first if(TemporalCoherenceEnabled() && FirstContactEnabled()) { PrimTest(cache->id0, cache->id1); if(GetContactStatus()) return true; } return false; } #define UPDATE_CACHE \ if(cache && GetContactStatus()) \ { \ cache->id0 = mPairs.GetEntry(0); \ cache->id1 = mPairs.GetEntry(1); \ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Collision query for normal AABB trees. * \param tree0 [in] AABB tree from first object * \param tree1 [in] AABB tree from second object * \param world0 [in] world matrix for first object * \param world1 [in] world matrix for second object * \param cache [in/out] cache for a pair of previously colliding primitives * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) { // Init collision query InitQuery(world0, world1); // Check previous state if(CheckTemporalCoherence(cache)) return true; // Perform collision query _Collide(tree0->GetNodes(), tree1->GetNodes()); UPDATE_CACHE return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Collision query for no-leaf AABB trees. * \param tree0 [in] AABB tree from first object * \param tree1 [in] AABB tree from second object * \param world0 [in] world matrix for first object * \param world1 [in] world matrix for second object * \param cache [in/out] cache for a pair of previously colliding primitives * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) { // Init collision query InitQuery(world0, world1); // Check previous state if(CheckTemporalCoherence(cache)) return true; // Perform collision query _Collide(tree0->GetNodes(), tree1->GetNodes()); UPDATE_CACHE return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Collision query for quantized AABB trees. * \param tree0 [in] AABB tree from first object * \param tree1 [in] AABB tree from second object * \param world0 [in] world matrix for first object * \param world1 [in] world matrix for second object * \param cache [in/out] cache for a pair of previously colliding primitives * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) { // Init collision query InitQuery(world0, world1); // Check previous state if(CheckTemporalCoherence(cache)) return true; // Setup dequantization coeffs mCenterCoeff0 = tree0->mCenterCoeff; mExtentsCoeff0 = tree0->mExtentsCoeff; mCenterCoeff1 = tree1->mCenterCoeff; mExtentsCoeff1 = tree1->mExtentsCoeff; // Dequantize box A const AABBQuantizedNode* N0 = tree0->GetNodes(); const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z); const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z); // Dequantize box B const AABBQuantizedNode* N1 = tree1->GetNodes(); const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z); const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z); // Perform collision query _Collide(N0, N1, a, Pa, b, Pb); UPDATE_CACHE return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Collision query for quantized no-leaf AABB trees. * \param tree0 [in] AABB tree from first object * \param tree1 [in] AABB tree from second object * \param world0 [in] world matrix for first object * \param world1 [in] world matrix for second object * \param cache [in/out] cache for a pair of previously colliding primitives * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) { // Init collision query InitQuery(world0, world1); // Check previous state if(CheckTemporalCoherence(cache)) return true; // Setup dequantization coeffs mCenterCoeff0 = tree0->mCenterCoeff; mExtentsCoeff0 = tree0->mExtentsCoeff; mCenterCoeff1 = tree1->mCenterCoeff; mExtentsCoeff1 = tree1->mExtentsCoeff; // Perform collision query _Collide(tree0->GetNodes(), tree1->GetNodes()); UPDATE_CACHE return true; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Standard trees /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // The normal AABB tree can use 2 different descent rules (with different performances) //#define ORIGINAL_CODE //!< UNC-like descent rules #define ALTERNATIVE_CODE //!< Alternative descent rules #ifdef ORIGINAL_CODE /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for normal AABB trees. * \param b0 [in] collision node from first tree * \param b1 [in] collision node from second tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1) { mNowNode0 = b0; mNowNode1 = b1; // Perform BV-BV overlap test if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) return; if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) { _Collide(b0->GetNeg(), b1); if(ContactFound()) return; _Collide(b0->GetPos(), b1); } else { _Collide(b0, b1->GetNeg()); if(ContactFound()) return; _Collide(b0, b1->GetPos()); } } #endif #ifdef ALTERNATIVE_CODE /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for normal AABB trees. * \param b0 [in] collision node from first tree * \param b1 [in] collision node from second tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1) { // Perform BV-BV overlap test if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) { return; } if(b0->IsLeaf()) { if(b1->IsLeaf()) { mNowNode0 = b0; mNowNode1 = b1; PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); } else { _Collide(b0, b1->GetNeg()); if(ContactFound()) return; _Collide(b0, b1->GetPos()); } } else if(b1->IsLeaf()) { _Collide(b0->GetNeg(), b1); if(ContactFound()) return; _Collide(b0->GetPos(), b1); } else { _Collide(b0->GetNeg(), b1->GetNeg()); if(ContactFound()) return; _Collide(b0->GetNeg(), b1->GetPos()); if(ContactFound()) return; _Collide(b0->GetPos(), b1->GetNeg()); if(ContactFound()) return; _Collide(b0->GetPos(), b1->GetPos()); } } #endif /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // No-leaf trees /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Leaf-leaf test for two primitive indices. * \param id0 [in] index from first leaf-triangle * \param id1 [in] index from second leaf-triangle */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::PrimTest(udword id0, udword id1) { mId0 = id0; mId1 = id1; // Request vertices from the app VertexPointers VP0; VertexPointers VP1; mIMesh0->GetTriangle(VP0, id0); mIMesh1->GetTriangle(VP1, id1); // Modified by S-cubed, Inc. // Transform from space 0 (old : 1) to space 1 (old : 0) // CD $B$G$OJQ49$,5U$J$N$G$"$o$;$k!#(B Point u0,u1,u2; TransformPoint(u0, *VP0.Vertex[0], mR0to1, mT0to1); TransformPoint(u1, *VP0.Vertex[1], mR0to1, mT0to1); TransformPoint(u2, *VP0.Vertex[2], mR0to1, mT0to1); // Perform triangle-triangle overlap test if(TriTriOverlap(u0, u1, u2, *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2])) { // Keep track of colliding pairs mPairs.Add(id0).Add(id1); // Set contact status mFlags |= OPC_CONTACT; } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Leaf-leaf test for a previously fetched triangle from tree A (in B's space) and a new leaf from B. * \param id1 [in] leaf-triangle index from tree B */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1) { // Request vertices from the app VertexPointers VP; mIMesh1->GetTriangle(VP, id1); // Perform triangle-triangle overlap test if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) { // Keep track of colliding pairs mPairs.Add(mLeafIndex).Add(id1); // Set contact status mFlags |= OPC_CONTACT; } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Leaf-leaf test for a previously fetched triangle from tree B (in A's space) and a new leaf from A. * \param id0 [in] leaf-triangle index from tree A */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0) { // Request vertices from the app VertexPointers VP; mIMesh0->GetTriangle(VP, id0); // Perform triangle-triangle overlap test if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) { // Keep track of colliding pairs mPairs.Add(id0).Add(mLeafIndex); // Set contact status mFlags |= OPC_CONTACT; } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision of a leaf node from A and a branch from B. * \param b [in] collision node from second tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b) { // Perform triangle-box overlap test // Modified by S-cubed, Inc. //NoleafNode $B$O;HMQ$7$J$$(B // if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return; // Keep same triangle, deal with first child if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); else _CollideTriBox(b->GetPos()); if(ContactFound()) return; // Keep same triangle, deal with second child if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); else _CollideTriBox(b->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision of a leaf node from B and a branch from A. * \param b [in] collision node from first tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b) { // Modified by S-cubed, Inc. //NoleafNode $B$O;HMQ$7$J$$(B // Perform triangle-box overlap test //if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return; // Keep same triangle, deal with first child if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); else _CollideBoxTri(b->GetPos()); if(ContactFound()) return; // Keep same triangle, deal with second child if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); else _CollideBoxTri(b->GetNeg()); } //! Request triangle vertices from the app and transform them #define FETCH_LEAF(prim_index, imesh, rot, trans) \ mLeafIndex = prim_index; \ /* Request vertices from the app */ \ VertexPointers VP; imesh->GetTriangle(VP, prim_index); \ /* Transform them in a common space */ \ TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \ TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \ TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for no-leaf AABB trees. * \param a [in] collision node from first tree * \param b [in] collision node from second tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b) { // Perform BV-BV overlap test if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return; // Catch leaf status BOOL BHasPosLeaf = b->HasPosLeaf(); BOOL BHasNegLeaf = b->HasNegLeaf(); if(a->HasPosLeaf()) { FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); else _CollideTriBox(b->GetPos()); if(ContactFound()) return; if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); else _CollideTriBox(b->GetNeg()); } else { if(BHasPosLeaf) { FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) _CollideBoxTri(a->GetPos()); } else _Collide(a->GetPos(), b->GetPos()); if(ContactFound()) return; if(BHasNegLeaf) { FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) _CollideBoxTri(a->GetPos()); } else _Collide(a->GetPos(), b->GetNeg()); } if(ContactFound()) return; if(a->HasNegLeaf()) { FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); else _CollideTriBox(b->GetPos()); if(ContactFound()) return; if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); else _CollideTriBox(b->GetNeg()); } else { if(BHasPosLeaf) { // ### That leaf has possibly already been fetched FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) _CollideBoxTri(a->GetNeg()); } else _Collide(a->GetNeg(), b->GetPos()); if(ContactFound()) return; if(BHasNegLeaf) { // ### That leaf has possibly already been fetched FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) _CollideBoxTri(a->GetNeg()); } else _Collide(a->GetNeg(), b->GetNeg()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Quantized trees /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for quantized AABB trees. * \param b0 [in] collision node from first tree * \param b1 [in] collision node from second tree * \param a [in] extent from box A * \param Pa [in] center from box A * \param b [in] extent from box B * \param Pb [in] center from box B */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb) { // Perform BV-BV overlap test if(!BoxBoxOverlap(a, Pa, b, Pb)) return; if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) { // Dequantize box const QuantizedAABB* Box = &b0->GetNeg()->mAABB; const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb); if(ContactFound()) return; // Dequantize box Box = &b0->GetPos()->mAABB; const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); _Collide(b0->GetPos(), b1, posa, posPa, b, Pb); } else { // Dequantize box const QuantizedAABB* Box = &b1->GetNeg()->mAABB; const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb); if(ContactFound()) return; // Dequantize box Box = &b1->GetPos()->mAABB; const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); _Collide(b0, b1->GetPos(), a, Pa, posb, posPb); } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Quantized no-leaf trees /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision of a leaf node from A and a quantized branch from B. * \param leaf [in] leaf triangle from first tree * \param b [in] collision node from second tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b) { // Dequantize box const QuantizedAABB* bb = &b->mAABB; const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); // Perform triangle-box overlap test // Modified by S-cubed, Inc. //NoleafNode $B$O;HMQ$7$J$$(B //if(!TriBoxOverlap(Pb, eb)) return; if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); else _CollideTriBox(b->GetPos()); if(ContactFound()) return; if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); else _CollideTriBox(b->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision of a leaf node from B and a quantized branch from A. * \param b [in] collision node from first tree * \param leaf [in] leaf triangle from second tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b) { // Dequantize box const QuantizedAABB* bb = &b->mAABB; const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z); const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z); // Modified by S-cubed, Inc. //NoleafNode $B$O;HMQ$7$J$$(B // Perform triangle-box overlap test // if(!TriBoxOverlap(Pa, ea)) return; if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); else _CollideBoxTri(b->GetPos()); if(ContactFound()) return; if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); else _CollideBoxTri(b->GetNeg()); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Recursive collision query for quantized no-leaf AABB trees. * \param a [in] collision node from first tree * \param b [in] collision node from second tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b) { // Dequantize box A const QuantizedAABB* ab = &a->mAABB; const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z); const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z); // Dequantize box B const QuantizedAABB* bb = &b->mAABB; const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); // Perform BV-BV overlap test if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return; // Catch leaf status BOOL BHasPosLeaf = b->HasPosLeaf(); BOOL BHasNegLeaf = b->HasNegLeaf(); if(a->HasPosLeaf()) { FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); else _CollideTriBox(b->GetPos()); if(ContactFound()) return; if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); else _CollideTriBox(b->GetNeg()); } else { if(BHasPosLeaf) { FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) _CollideBoxTri(a->GetPos()); } else _Collide(a->GetPos(), b->GetPos()); if(ContactFound()) return; if(BHasNegLeaf) { FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) _CollideBoxTri(a->GetPos()); } else _Collide(a->GetPos(), b->GetNeg()); } if(ContactFound()) return; if(a->HasNegLeaf()) { FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); else _CollideTriBox(b->GetPos()); if(ContactFound()) return; if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); else _CollideTriBox(b->GetNeg()); } else { if(BHasPosLeaf) { // ### That leaf has possibly already been fetched FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) _CollideBoxTri(a->GetNeg()); } else _Collide(a->GetNeg(), b->GetPos()); if(ContactFound()) return; if(BHasNegLeaf) { // ### That leaf has possibly already been fetched FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) _CollideBoxTri(a->GetNeg()); } else _Collide(a->GetNeg(), b->GetNeg()); } } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_TreeCollider.h000066400000000000000000000331631207742442300237560ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains code for a tree collider. * \file OPC_TreeCollider.h * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_TREECOLLIDER_H__ #define __OPC_TREECOLLIDER_H__ //! This structure holds cached information used by the algorithm. //! Two model pointers and two colliding primitives are cached. Model pointers are assigned //! to their respective meshes, and the pair of colliding primitives is used for temporal //! coherence. That is, in case temporal coherence is enabled, those two primitives are //! tested for overlap before everything else. If they still collide, we're done before //! even entering the recursive collision code. struct OPCODE_API BVTCache : Pair { //! Constructor inline_ BVTCache() { ResetCache(); ResetCountDown(); } void ResetCache() { Model0 = null; Model1 = null; id0 = 0; id1 = 1; #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! HullTest = true; SepVector.pid = 0; SepVector.qid = 0; SepVector.SV = Point(1.0f, 0.0f, 0.0f); #endif // __MESHMERIZER_H__ } inline_ void ResetCountDown() { #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! CountDown = 50; #endif // __MESHMERIZER_H__ } const Model* Model0; //!< Model for first object const Model* Model1; //!< Model for second object #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! SVCache SepVector; udword CountDown; bool HullTest; #endif // __MESHMERIZER_H__ }; class OPCODE_API AABBTreeCollider : public Collider { public: // Constructor / Destructor AABBTreeCollider(); virtual ~AABBTreeCollider(); inline void setCollisionPairInserter(cnoid::CollisionPairInserter* collisionPairInserter) { this->collisionPairInserter = collisionPairInserter; } // Generic collision query /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Generic collision query for generic OPCODE models. After the call, access the results with: * - GetContactStatus() * - GetNbPairs() * - GetPairs() * * \param cache [in] collision cache for model pointers and a colliding pair of primitives * \param world0 [in] world matrix for first object, or null * \param world1 [in] world matrix for second object, or null * \return true if success * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Collide(BVTCache& cache, const Matrix4x4* world0=null, const Matrix4x4* world1=null); // Collision queries bool Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); bool Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); bool Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); bool Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); // Settings /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Settings: selects between full box-box tests or "SAT-lite" tests (where Class III axes are discarded) * \param flag [in] true for full tests, false for coarse tests * \see SetFullPrimBoxTest(bool flag) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetFullBoxBoxTest(bool flag) { mFullBoxBoxTest = flag; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Settings: selects between full triangle-box tests or "SAT-lite" tests (where Class III axes are discarded) * \param flag [in] true for full tests, false for coarse tests * \see SetFullBoxBoxTest(bool flag) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ void SetFullPrimBoxTest(bool flag) { mFullPrimBoxTest = flag; } // Stats /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stats: gets the number of BV-BV overlap tests after a collision query. * \see GetNbPrimPrimTests() * \see GetNbBVPrimTests() * \return the number of BV-BV tests performed during last query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbBVBVTests() const { return mNbBVBVTests; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stats: gets the number of Triangle-Triangle overlap tests after a collision query. * \see GetNbBVBVTests() * \see GetNbBVPrimTests() * \return the number of Triangle-Triangle tests performed during last query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbPrimPrimTests() const { return mNbPrimPrimTests; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stats: gets the number of BV-Triangle overlap tests after a collision query. * \see GetNbBVBVTests() * \see GetNbPrimPrimTests() * \return the number of BV-Triangle tests performed during last query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbBVPrimTests() const { return mNbBVPrimTests; } // Data access /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of contacts after a collision query. * \see GetContactStatus() * \see GetPairs() * \return the number of contacts / colliding pairs. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbPairs() const { return mPairs.GetNbEntries()>>1; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the pairs of colliding triangles after a collision query. * \see GetContactStatus() * \see GetNbPairs() * \return the list of colliding pairs (triangle indices) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ const Pair* GetPairs() const { return (const Pair*)mPairs.GetEntries(); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider. * \return null if everything is ok, else a string describing the problem */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(Collider) const char* ValidateSettings(); protected: // Colliding pairs Container mPairs; //!< Pairs of colliding primitives // User mesh interfaces const MeshInterface* mIMesh0; //!< User-defined mesh interface for object0 const MeshInterface* mIMesh1; //!< User-defined mesh interface for object1 // Stats udword mNbBVBVTests; //!< Number of BV-BV tests udword mNbPrimPrimTests; //!< Number of Primitive-Primitive tests udword mNbBVPrimTests; //!< Number of BV-Primitive tests // Precomputed data Matrix3x3 mAR; //!< Absolute rotation matrix Matrix3x3 mR0to1; //!< Rotation from object0 to object1 Matrix3x3 mR1to0; //!< Rotation from object1 to object0 Point mT0to1; //!< Translation from object0 to object1 Point mT1to0; //!< Translation from object1 to object0 // Dequantization coeffs Point mCenterCoeff0; Point mExtentsCoeff0; Point mCenterCoeff1; Point mExtentsCoeff1; // Modified!! // For normal vector detection udword mId0; udword mId1; const AABBCollisionNode* mNowNode0; const AABBCollisionNode* mNowNode1; // Leaf description Point mLeafVerts[3]; //!< Triangle vertices udword mLeafIndex; //!< Triangle index // Settings bool mFullBoxBoxTest; //!< Perform full BV-BV tests (true) or SAT-lite tests (false) bool mFullPrimBoxTest; //!< Perform full Primitive-BV tests (true) or SAT-lite tests (false) cnoid::CollisionPairInserter* collisionPairInserter; // Internal methods // Standard AABB trees void _Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1); // Quantized AABB trees void _Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb); // No-leaf AABB trees void _CollideTriBox(const AABBNoLeafNode* b); void _CollideBoxTri(const AABBNoLeafNode* b); void _Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b); // Quantized no-leaf AABB trees void _CollideTriBox(const AABBQuantizedNoLeafNode* b); void _CollideBoxTri(const AABBQuantizedNoLeafNode* b); void _Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b); // Overlap tests void PrimTest(udword id0, udword id1); inline_ void PrimTestTriIndex(udword id1); inline_ void PrimTestIndexTri(udword id0); inline_ BOOL BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb); inline_ BOOL TriBoxOverlap(const Point& center, const Point& extents); BOOL TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2); // Init methods void InitQuery(const Matrix4x4* world0=null, const Matrix4x4* world1=null); bool CheckTemporalCoherence(Pair* cache); inline_ BOOL Setup(const MeshInterface* mi0, const MeshInterface* mi1) { mIMesh0 = mi0; mIMesh1 = mi1; if(!mIMesh0 || !mIMesh1) return FALSE; return TRUE; } }; #endif // __OPC_TREECOLLIDER_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_TriBoxOverlap.h000066400000000000000000000263331207742442300241420ustar00rootroot00000000000000 //! This macro quickly finds the min & max values among 3 variables #define FINDMINMAX(x0, x1, x2, min, max) \ min = max = x0; \ if(x1max) max=x1; \ if(x2max) max=x2; //! TO BE DOCUMENTED inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox) { Point vmin, vmax; for(udword q=0;q<=2;q++) { if(normal[q]>0.0f) { vmin[q]=-maxbox[q]; vmax[q]=maxbox[q]; } else { vmin[q]=maxbox[q]; vmax[q]=-maxbox[q]; } } if((normal|vmin)+d>0.0f) return FALSE; if((normal|vmax)+d>=0.0f) return TRUE; return FALSE; } //! TO BE DOCUMENTED #define AXISTEST_X01(a, b, fa, fb) \ min = a*v0.y - b*v0.z; \ max = a*v2.y - b*v2.z; \ if(min>max) {const float tmp=max; max=min; min=tmp; } \ rad = fa * extents.y + fb * extents.z; \ if(min>rad || max<-rad) return FALSE; //! TO BE DOCUMENTED #define AXISTEST_X2(a, b, fa, fb) \ min = a*v0.y - b*v0.z; \ max = a*v1.y - b*v1.z; \ if(min>max) {const float tmp=max; max=min; min=tmp; } \ rad = fa * extents.y + fb * extents.z; \ if(min>rad || max<-rad) return FALSE; //! TO BE DOCUMENTED #define AXISTEST_Y02(a, b, fa, fb) \ min = b*v0.z - a*v0.x; \ max = b*v2.z - a*v2.x; \ if(min>max) {const float tmp=max; max=min; min=tmp; } \ rad = fa * extents.x + fb * extents.z; \ if(min>rad || max<-rad) return FALSE; //! TO BE DOCUMENTED #define AXISTEST_Y1(a, b, fa, fb) \ min = b*v0.z - a*v0.x; \ max = b*v1.z - a*v1.x; \ if(min>max) {const float tmp=max; max=min; min=tmp; } \ rad = fa * extents.x + fb * extents.z; \ if(min>rad || max<-rad) return FALSE; //! TO BE DOCUMENTED #define AXISTEST_Z12(a, b, fa, fb) \ min = a*v1.x - b*v1.y; \ max = a*v2.x - b*v2.y; \ if(min>max) {const float tmp=max; max=min; min=tmp; } \ rad = fa * extents.x + fb * extents.y; \ if(min>rad || max<-rad) return FALSE; //! TO BE DOCUMENTED #define AXISTEST_Z0(a, b, fa, fb) \ min = a*v0.x - b*v0.y; \ max = a*v1.x - b*v1.y; \ if(min>max) {const float tmp=max; max=min; min=tmp; } \ rad = fa * extents.x + fb * extents.y; \ if(min>rad || max<-rad) return FALSE; // compute triangle edges // - edges lazy evaluated to take advantage of early exits // - fabs precomputed (half less work, possible since extents are always >0) // - customized macros to take advantage of the null component // - axis vector discarded, possibly saves useless movs #define IMPLEMENT_CLASS3_TESTS \ float rad; \ float min, max; \ \ const float fey0 = fabsf(e0.y); \ const float fez0 = fabsf(e0.z); \ AXISTEST_X01(e0.z, e0.y, fez0, fey0); \ const float fex0 = fabsf(e0.x); \ AXISTEST_Y02(e0.z, e0.x, fez0, fex0); \ AXISTEST_Z12(e0.y, e0.x, fey0, fex0); \ \ const float fey1 = fabsf(e1.y); \ const float fez1 = fabsf(e1.z); \ AXISTEST_X01(e1.z, e1.y, fez1, fey1); \ const float fex1 = fabsf(e1.x); \ AXISTEST_Y02(e1.z, e1.x, fez1, fex1); \ AXISTEST_Z0(e1.y, e1.x, fey1, fex1); \ \ const Point e2 = mLeafVerts[0] - mLeafVerts[2]; \ const float fey2 = fabsf(e2.y); \ const float fez2 = fabsf(e2.z); \ AXISTEST_X2(e2.z, e2.y, fez2, fey2); \ const float fex2 = fabsf(e2.x); \ AXISTEST_Y1(e2.z, e2.x, fez2, fex2); \ AXISTEST_Z12(e2.y, e2.x, fey2, fex2); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Triangle-Box overlap test using the separating axis theorem. * This is the code from Tomas Möller, a bit optimized: * - with some more lazy evaluation (faster path on PC) * - with a tiny bit of assembly * - with "SAT-lite" applied if needed * - and perhaps with some more minor modifs... * * \param center [in] box center * \param extents [in] box extents * \return true if triangle & box overlap */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& extents) { // Stats mNbBVPrimTests++; // use separating axis theorem to test overlap between triangle and box // need to test for overlap in these directions: // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle // we do not even need to test these) // 2) normal of the triangle // 3) crossproduct(edge from tri, {x,y,z}-directin) // this gives 3x3=9 more tests // move everything so that the boxcenter is in (0,0,0) Point v0, v1, v2; v0.x = mLeafVerts[0].x - center.x; v1.x = mLeafVerts[1].x - center.x; v2.x = mLeafVerts[2].x - center.x; // First, test overlap in the {x,y,z}-directions #ifdef OPC_USE_FCOMI // find min, max of the triangle in x-direction, and test for overlap in X if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE; if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE; // same for Y v0.y = mLeafVerts[0].y - center.y; v1.y = mLeafVerts[1].y - center.y; v2.y = mLeafVerts[2].y - center.y; if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE; if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE; // same for Z v0.z = mLeafVerts[0].z - center.z; v1.z = mLeafVerts[1].z - center.z; v2.z = mLeafVerts[2].z - center.z; if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE; if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE; #else float min,max; // Find min, max of the triangle in x-direction, and test for overlap in X FINDMINMAX(v0.x, v1.x, v2.x, min, max); if(min>extents.x || max<-extents.x) return FALSE; // Same for Y v0.y = mLeafVerts[0].y - center.y; v1.y = mLeafVerts[1].y - center.y; v2.y = mLeafVerts[2].y - center.y; FINDMINMAX(v0.y, v1.y, v2.y, min, max); if(min>extents.y || max<-extents.y) return FALSE; // Same for Z v0.z = mLeafVerts[0].z - center.z; v1.z = mLeafVerts[1].z - center.z; v2.z = mLeafVerts[2].z - center.z; FINDMINMAX(v0.z, v1.z, v2.z, min, max); if(min>extents.z || max<-extents.z) return FALSE; #endif // 2) Test if the box intersects the plane of the triangle // compute plane equation of triangle: normal*x+d=0 // ### could be precomputed since we use the same leaf triangle several times const Point e0 = v1 - v0; const Point e1 = v2 - v1; const Point normal = e0 ^ e1; const float d = -normal|v0; if(!planeBoxOverlap(normal, d, extents)) return FALSE; // 3) "Class III" tests if(mFullPrimBoxTest) { IMPLEMENT_CLASS3_TESTS } return TRUE; } //! A dedicated version where the box is constant inline_ BOOL OBBCollider::TriBoxOverlap() { // Stats mNbVolumePrimTests++; // Hook const Point& extents = mBoxExtents; const Point& v0 = mLeafVerts[0]; const Point& v1 = mLeafVerts[1]; const Point& v2 = mLeafVerts[2]; // use separating axis theorem to test overlap between triangle and box // need to test for overlap in these directions: // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle // we do not even need to test these) // 2) normal of the triangle // 3) crossproduct(edge from tri, {x,y,z}-directin) // this gives 3x3=9 more tests // Box center is already in (0,0,0) // First, test overlap in the {x,y,z}-directions #ifdef OPC_USE_FCOMI // find min, max of the triangle in x-direction, and test for overlap in X if(FCMin3(v0.x, v1.x, v2.x)>mBoxExtents.x) return FALSE; if(FCMax3(v0.x, v1.x, v2.x)<-mBoxExtents.x) return FALSE; if(FCMin3(v0.y, v1.y, v2.y)>mBoxExtents.y) return FALSE; if(FCMax3(v0.y, v1.y, v2.y)<-mBoxExtents.y) return FALSE; if(FCMin3(v0.z, v1.z, v2.z)>mBoxExtents.z) return FALSE; if(FCMax3(v0.z, v1.z, v2.z)<-mBoxExtents.z) return FALSE; #else float min,max; // Find min, max of the triangle in x-direction, and test for overlap in X FINDMINMAX(v0.x, v1.x, v2.x, min, max); if(min>mBoxExtents.x || max<-mBoxExtents.x) return FALSE; FINDMINMAX(v0.y, v1.y, v2.y, min, max); if(min>mBoxExtents.y || max<-mBoxExtents.y) return FALSE; FINDMINMAX(v0.z, v1.z, v2.z, min, max); if(min>mBoxExtents.z || max<-mBoxExtents.z) return FALSE; #endif // 2) Test if the box intersects the plane of the triangle // compute plane equation of triangle: normal*x+d=0 // ### could be precomputed since we use the same leaf triangle several times const Point e0 = v1 - v0; const Point e1 = v2 - v1; const Point normal = e0 ^ e1; const float d = -normal|v0; if(!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE; // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV) { IMPLEMENT_CLASS3_TESTS } return TRUE; } //! ...and another one, jeez inline_ BOOL AABBCollider::TriBoxOverlap() { // Stats mNbVolumePrimTests++; // Hook const Point& center = mBox.mCenter; const Point& extents = mBox.mExtents; // use separating axis theorem to test overlap between triangle and box // need to test for overlap in these directions: // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle // we do not even need to test these) // 2) normal of the triangle // 3) crossproduct(edge from tri, {x,y,z}-directin) // this gives 3x3=9 more tests // move everything so that the boxcenter is in (0,0,0) Point v0, v1, v2; v0.x = mLeafVerts[0].x - center.x; v1.x = mLeafVerts[1].x - center.x; v2.x = mLeafVerts[2].x - center.x; // First, test overlap in the {x,y,z}-directions #ifdef OPC_USE_FCOMI // find min, max of the triangle in x-direction, and test for overlap in X if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE; if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE; // same for Y v0.y = mLeafVerts[0].y - center.y; v1.y = mLeafVerts[1].y - center.y; v2.y = mLeafVerts[2].y - center.y; if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE; if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE; // same for Z v0.z = mLeafVerts[0].z - center.z; v1.z = mLeafVerts[1].z - center.z; v2.z = mLeafVerts[2].z - center.z; if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE; if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE; #else float min,max; // Find min, max of the triangle in x-direction, and test for overlap in X FINDMINMAX(v0.x, v1.x, v2.x, min, max); if(min>extents.x || max<-extents.x) return FALSE; // Same for Y v0.y = mLeafVerts[0].y - center.y; v1.y = mLeafVerts[1].y - center.y; v2.y = mLeafVerts[2].y - center.y; FINDMINMAX(v0.y, v1.y, v2.y, min, max); if(min>extents.y || max<-extents.y) return FALSE; // Same for Z v0.z = mLeafVerts[0].z - center.z; v1.z = mLeafVerts[1].z - center.z; v2.z = mLeafVerts[2].z - center.z; FINDMINMAX(v0.z, v1.z, v2.z, min, max); if(min>extents.z || max<-extents.z) return FALSE; #endif // 2) Test if the box intersects the plane of the triangle // compute plane equation of triangle: normal*x+d=0 // ### could be precomputed since we use the same leaf triangle several times const Point e0 = v1 - v0; const Point e1 = v2 - v1; const Point normal = e0 ^ e1; const float d = -normal|v0; if(!planeBoxOverlap(normal, d, extents)) return FALSE; // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV) { IMPLEMENT_CLASS3_TESTS } return TRUE; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_TriTriOverlap.h000066400000000000000000000167701207742442300241540ustar00rootroot00000000000000 //! if OPC_TRITRI_EPSILON_TEST is true then we do a check (if |dv|b) \ { \ const float c=a; \ a=b; \ b=c; \ } //! Edge to edge test based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202 #define EDGE_EDGE_TEST(V0, U0, U1) \ Bx = U0[i0] - U1[i0]; \ By = U0[i1] - U1[i1]; \ Cx = V0[i0] - U0[i0]; \ Cy = V0[i1] - U0[i1]; \ f = Ay*Bx - Ax*By; \ d = By*Cx - Bx*Cy; \ if((f>0.0f && d>=0.0f && d<=f) || (f<0.0f && d<=0.0f && d>=f)) \ { \ const float e=Ax*Cy - Ay*Cx; \ if(f>0.0f) \ { \ if(e>=0.0f && e<=f) return TRUE; \ } \ else \ { \ if(e<=0.0f && e>=f) return TRUE; \ } \ } //! TO BE DOCUMENTED #define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2) \ { \ float Bx,By,Cx,Cy,d,f; \ const float Ax = V1[i0] - V0[i0]; \ const float Ay = V1[i1] - V0[i1]; \ /* test edge U0,U1 against V0,V1 */ \ EDGE_EDGE_TEST(V0, U0, U1); \ /* test edge U1,U2 against V0,V1 */ \ EDGE_EDGE_TEST(V0, U1, U2); \ /* test edge U2,U1 against V0,V1 */ \ EDGE_EDGE_TEST(V0, U2, U0); \ } //! TO BE DOCUMENTED #define POINT_IN_TRI(V0, U0, U1, U2) \ { \ /* is T1 completly inside T2? */ \ /* check if V0 is inside tri(U0,U1,U2) */ \ float a = U1[i1] - U0[i1]; \ float b = -(U1[i0] - U0[i0]); \ float c = -a*U0[i0] - b*U0[i1]; \ float d0 = a*V0[i0] + b*V0[i1] + c; \ \ a = U2[i1] - U1[i1]; \ b = -(U2[i0] - U1[i0]); \ c = -a*U1[i0] - b*U1[i1]; \ const float d1 = a*V0[i0] + b*V0[i1] + c; \ \ a = U0[i1] - U2[i1]; \ b = -(U0[i0] - U2[i0]); \ c = -a*U2[i0] - b*U2[i1]; \ const float d2 = a*V0[i0] + b*V0[i1] + c; \ if(d0*d1>0.0f) \ { \ if(d0*d2>0.0f) return TRUE; \ } \ } //! TO BE DOCUMENTED BOOL CoplanarTriTri(const Point& n, const Point& v0, const Point& v1, const Point& v2, const Point& u0, const Point& u1, const Point& u2) { float A[3]; short i0,i1; /* first project onto an axis-aligned plane, that maximizes the area */ /* of the triangles, compute indices: i0,i1. */ A[0] = fabsf(n[0]); A[1] = fabsf(n[1]); A[2] = fabsf(n[2]); if(A[0]>A[1]) { if(A[0]>A[2]) { i0=1; /* A[0] is greatest */ i1=2; } else { i0=0; /* A[2] is greatest */ i1=1; } } else /* A[0]<=A[1] */ { if(A[2]>A[1]) { i0=0; /* A[2] is greatest */ i1=1; } else { i0=0; /* A[1] is greatest */ i1=2; } } /* test all edges of triangle 1 against the edges of triangle 2 */ EDGE_AGAINST_TRI_EDGES(v0, v1, u0, u1, u2); EDGE_AGAINST_TRI_EDGES(v1, v2, u0, u1, u2); EDGE_AGAINST_TRI_EDGES(v2, v0, u0, u1, u2); /* finally, test if tri1 is totally contained in tri2 or vice versa */ POINT_IN_TRI(v0, u0, u1, u2); POINT_IN_TRI(u0, v0, v1, v2); return FALSE; } //! TO BE DOCUMENTED #define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, X0, X1) \ { \ if(D0D1>0.0f) \ { \ /* here we know that D0D2<=0.0 */ \ /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1; \ } \ else if(D0D2>0.0f) \ { \ /* here we know that d0d1<=0.0 */ \ A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2; \ } \ else if(D1*D2>0.0f || D0!=0.0f) \ { \ /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ A=VV0; B=(VV1 - VV0)*D0; C=(VV2 - VV0)*D0; X0=D0 - D1; X1=D0 - D2; \ } \ else if(D1!=0.0f) \ { \ A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2; \ } \ else if(D2!=0.0f) \ { \ A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1; \ } \ else \ { \ /* triangles are coplanar */ \ return CoplanarTriTri(N1, V0, V1, V2, U0, U1, U2); \ } \ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Triangle/triangle intersection test routine, * by Tomas Moller, 1997. * See article "A Fast Triangle-Triangle Intersection Test", * Journal of Graphics Tools, 2(2), 1997 * * Updated June 1999: removed the divisions -- a little faster now! * Updated October 1999: added {} to CROSS and SUB macros * * int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3], * float U0[3],float U1[3],float U2[3]) * * \param V0 [in] triangle 0, vertex 0 * \param V1 [in] triangle 0, vertex 1 * \param V2 [in] triangle 0, vertex 2 * \param U0 [in] triangle 1, vertex 0 * \param U1 [in] triangle 1, vertex 1 * \param U2 [in] triangle 1, vertex 2 * \return true if triangles overlap */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2) { // Stats mNbPrimPrimTests++; // Modified by S-cubed, Inc. // Detect collisions by using TriOverlap.cpp instead of the collision detection by OPCODE cnoid::collision_data c_pair; cnoid::Vector3 i0, i1, i2; cnoid::Vector3 p0, p1, p2; // Convert coordinates to match the interface of tri_tri_overlap()@TriOerlap.cpp // Ice/IcePointer => Vector3 i0[0] = V0.x; i0[1] = V0.y; i0[2] = V0.z; i1[0] = V1.x; i1[1] = V1.y; i1[2] = V1.z; i2[0] = V2.x; i2[1] = V2.y; i2[2] = V2.z; p0[0] = U0.x; p0[1] = U0.y; p0[2] = U0.z; p1[0] = U1.x; p1[1] = U1.y; p1[2] = U1.z; p2[0] = U2.x; p2[1] = U2.y; p2[2] = U2.z; if(collisionPairInserter && collisionPairInserter->detectTriTriOverlap(i0, i1, i2, p0, p1, p2, &c_pair)){ /* insert_collision_pair(mNowNode0, mNowNode1, mId0, mId1, c_pair.num_of_i_points, c_pair.i_points, c_pair.n_vector, c_pair.depth, c_pair.n, c_pair.m, c_pair.c_type, (MeshInterface*)mIMesh0, (MeshInterface*)mIMesh1); */ collisionPairInserter->apply(mNowNode0, mNowNode1, mId0, mId1, c_pair.num_of_i_points, c_pair.i_points, c_pair.n_vector, c_pair.depth, c_pair.n, c_pair.m, c_pair.c_type, (MeshInterface*)mIMesh0, (MeshInterface*)mIMesh1); return TRUE; } return FALSE; } choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_VolumeCollider.cpp000066400000000000000000000111131207742442300246500ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains base volume collider class. * \file OPC_VolumeCollider.cpp * \author Pierre Terdiman * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains the abstract class for volume colliders. * * \class VolumeCollider * \author Pierre Terdiman * \version 1.3 * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" using namespace Opcode; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// VolumeCollider::VolumeCollider() : mTouchedPrimitives (null), mNbVolumeBVTests (0), mNbVolumePrimTests (0) { } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// VolumeCollider::~VolumeCollider() { mTouchedPrimitives = null; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider. * \return null if everything is ok, else a string describing the problem */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// const char* VolumeCollider::ValidateSettings() { return null; } // Pretty dumb way to dump - to do better - one day... #define IMPLEMENT_NOLEAFDUMP(type) \ void VolumeCollider::_Dump(const type* node) \ { \ if(node->HasPosLeaf()) mTouchedPrimitives->Add(node->GetPosPrimitive()); \ else _Dump(node->GetPos()); \ \ if(ContactFound()) return; \ \ if(node->HasNegLeaf()) mTouchedPrimitives->Add(node->GetNegPrimitive()); \ else _Dump(node->GetNeg()); \ } #define IMPLEMENT_LEAFDUMP(type) \ void VolumeCollider::_Dump(const type* node) \ { \ if(node->IsLeaf()) \ { \ mTouchedPrimitives->Add(node->GetPrimitive()); \ } \ else \ { \ _Dump(node->GetPos()); \ \ if(ContactFound()) return; \ \ _Dump(node->GetNeg()); \ } \ } IMPLEMENT_NOLEAFDUMP(AABBNoLeafNode) IMPLEMENT_NOLEAFDUMP(AABBQuantizedNoLeafNode) IMPLEMENT_LEAFDUMP(AABBCollisionNode) IMPLEMENT_LEAFDUMP(AABBQuantizedNode) choreonoid-1.1.0+dfsg/src/Collision/Opcode/OPC_VolumeCollider.h000066400000000000000000000157231207742442300243300ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Contains base volume collider class. * \file OPC_VolumeCollider.h * \author Pierre Terdiman * \date June, 2, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPC_VOLUMECOLLIDER_H__ #define __OPC_VOLUMECOLLIDER_H__ struct OPCODE_API VolumeCache { VolumeCache() : Model(null) {} ~VolumeCache() {} Container TouchedPrimitives; //!< Indices of touched primitives const BaseModel* Model; //!< Owner }; class OPCODE_API VolumeCollider : public Collider { public: // Constructor / Destructor VolumeCollider(); virtual ~VolumeCollider() = 0; // Collision report /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of touched primitives after a collision query. * \see GetContactStatus() * \see GetTouchedPrimitives() * \return the number of touched primitives */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbTouchedPrimitives() const { return mTouchedPrimitives ? mTouchedPrimitives->GetNbEntries() : 0; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the list of touched primitives after a collision query. * \see GetContactStatus() * \see GetNbTouchedPrimitives() * \return the list of touched primitives (primitive indices) */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ const udword* GetTouchedPrimitives() const { return mTouchedPrimitives ? mTouchedPrimitives->GetEntries() : null; } // Stats /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stats: gets the number of Volume-BV overlap tests after a collision query. * \see GetNbVolumePrimTests() * \return the number of Volume-BV tests performed during last query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbVolumeBVTests() const { return mNbVolumeBVTests; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Stats: gets the number of Volume-Triangle overlap tests after a collision query. * \see GetNbVolumeBVTests() * \return the number of Volume-Triangle tests performed during last query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbVolumePrimTests() const { return mNbVolumePrimTests; } // Settings /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider. * \return null if everything is ok, else a string describing the problem */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(Collider) const char* ValidateSettings(); protected: // Touched primitives Container* mTouchedPrimitives; //!< List of touched primitives // Dequantization coeffs Point mCenterCoeff; Point mExtentsCoeff; // Stats udword mNbVolumeBVTests; //!< Number of Volume-BV tests udword mNbVolumePrimTests; //!< Number of Volume-Primitive tests // Internal methods void _Dump(const AABBCollisionNode* node); void _Dump(const AABBNoLeafNode* node); void _Dump(const AABBQuantizedNode* node); void _Dump(const AABBQuantizedNoLeafNode* node); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Initializes a query */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(Collider) inline_ void InitQuery() { // Reset stats & contact status mNbVolumeBVTests = 0; mNbVolumePrimTests = 0; Collider::InitQuery(); } inline_ BOOL IsCacheValid(VolumeCache& cache) { // We're going to do a volume-vs-model query. if(cache.Model!=mCurrentModel) { // Cached list was for another model so we can't keep it // Keep track of new owner and reset cache cache.Model = mCurrentModel; return FALSE; } else { // Same models, no problem return TRUE; } } }; #endif // __OPC_VOLUMECOLLIDER_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/Opcode.cpp000066400000000000000000000041011207742442300224320ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Main file for Opcode.dll. * \file Opcode.cpp * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* Finding a good name is difficult! Here's the draft for this lib.... Spooky, uh? VOID? Very Optimized Interference Detection ZOID? Zappy's Optimized Interference Detection CID? Custom/Clever Interference Detection AID / ACID! Accurate Interference Detection QUID? Quick Interference Detection RIDE? Realtime Interference DEtection WIDE? Wicked Interference DEtection (....) GUID! KID ! k-dop interference detection :) OPCODE! OPtimized COllision DEtection */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header #include "Stdafx.h" bool Opcode::InitOpcode() { Log("// Initializing OPCODE\n\n"); // LogAPIInfo(); return true; } void ReleasePruningSorters(); bool Opcode::CloseOpcode() { Log("// Closing OPCODE\n\n"); ReleasePruningSorters(); return true; } #ifdef ICE_MAIN void ModuleAttach(HINSTANCE hinstance) { } void ModuleDetach() { } #endif choreonoid-1.1.0+dfsg/src/Collision/Opcode/Opcode.h000066400000000000000000000076651207742442300221210ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Main file for Opcode.dll. * \file Opcode.h * \author Pierre Terdiman * \date March, 20, 2001 */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Include Guard #ifndef __OPCODE_H__ #define __OPCODE_H__ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Things to help us compile on non-windows platforms #if defined(__APPLE__) || defined(__MACOSX__) #if __APPLE_CC__ < 1495 #define sqrtf sqrt #define sinf sin #define cosf cos #define acosf acos #define asinf sinf #endif #endif #ifndef _MSC_VER #define __int64 long long int #define __stdcall /* */ #endif /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Compilation messages #ifdef _MSC_VER #if defined(OPCODE_EXPORTS) // #pragma message("Compiling OPCODE") #elif !defined(OPCODE_EXPORTS) // #pragma message("Using OPCODE") /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Automatic linking #ifndef BAN_OPCODE_AUTOLINK #ifdef _DEBUG //#pragma comment(lib, "Opcode_D.lib") #else //#pragma comment(lib, "Opcode.lib") #endif #endif #endif #endif /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Preprocessor #ifndef ICE_NO_DLL #ifdef OPCODE_EXPORTS #define OPCODE_API// __declspec(dllexport) #else #define OPCODE_API// __declspec(dllimport) #endif #else #define OPCODE_API #endif #include "OPC_IceHook.h" //#include namespace cnoid { class CollisionPairInserter; } namespace Opcode { // Bulk-of-the-work #include "OPC_Settings.h" #include "OPC_Common.h" #include "OPC_MeshInterface.h" // Builders #include "OPC_TreeBuilders.h" // Trees #include "OPC_AABBTree.h" #include "OPC_OptimizedTree.h" // Models #include "OPC_BaseModel.h" #include "OPC_Model.h" #include "OPC_HybridModel.h" // Colliders #include "OPC_Collider.h" #include "OPC_VolumeCollider.h" #include "OPC_TreeCollider.h" #include "OPC_RayCollider.h" #include "OPC_SphereCollider.h" //#include "OPC_OBBCollider.h" //#include "OPC_AABBCollider.h" //#include "OPC_LSSCollider.h" //#include "OPC_PlanesCollider.h" // Usages #include "OPC_Picking.h" // Sweep-and-prune //#include "OPC_BoxPruning.h" //#include "OPC_SweepAndPrune.h" FUNCTION OPCODE_API bool InitOpcode(); FUNCTION OPCODE_API bool CloseOpcode(); } #endif // __OPCODE_H__ choreonoid-1.1.0+dfsg/src/Collision/Opcode/ReadMe.txt000066400000000000000000000210711207742442300224200ustar00rootroot00000000000000 OPCODE distribution 1.3 (june 2003) ----------------------- New in Opcode 1.3: - fixed the divide by 0 bug that was happening when all centers where located on a coordinate axis (thanks to Jorrit T) - linearized "complete" vanilla AABB trees - ANSI-compliant "for" loops (for the ones porting it to Linux...) - callbacks & pointers moved to mesh interface - support for triangle & vertex strides - optimized the sphere-triangle overlap code a bit - dynamic trees (refit) - more builders - ValidateSubdivision in builders - LSS collider - primitive-bv tests can now be skipped in most volume queries - temporal coherence now also works for airborne objects - temporal coherence completed for boxes / all contacts, LSS, etc - ray-collider now uses a callback - some common "usages" have been introduced (only picking for now) - SPLIT_COMPLETE removed (now implicitely using mLimit = 1) - hybrid collision models - sweep-and-prune code added, moved from my old Z-Collide lib - it now works with meshes made of only 1 triangle (except in mesh-mesh case!) Disclaimer: - I forced myself to actually *do* the release today no matter what. Else it would never have been done. That's why the code may not be very polished. I also removed a *lot* of things (more usages, distance queries, etc...) that weren't ready for prime-time (or that were linked to too many of my supporting libs) - Some comments may also be obsolete here and there. The old User Manual for Opcode 1.2 may not fit version 1.3 either, since there's a new "mesh interface" to support strides, etc. - Everything in the "Ice" directory has been hacked out of my engine and edited until everything compiled. Don't expect anything out there to be cute or something. In particular, some CPP files are not even included when not needed, so you can expect some linker errors if you try messing around with them... Otherwise, it should be just like previous version, only better. In particular, hybrid models can be very memory-friendly (sometimes using like 10 times less ram than the best trees from version 1.2). The possible speed hit is often invisible (if it even exists), especially using temporal coherence in "all contacts" mode. (Admittedly, this depends on your particular usage pattern / what you do on collided triangles). The sweep-and-prune code is similar to the "vanilla" version found in V-Collide (but that one's better IMHO...) The simple "radix" version is often just as good, see for yourself. OPCODE distribution 1.2 (august 2002) ----------------------- New in Opcode 1.2: - new VolumeCollider base class - simplified callback setup - you can now use callbacks or pointers (setup at compile time) - destination array not needed anymore in the RayCollider (faster in-out tests) - renamed classes: AABBRayCollider => RayCollider, AABBSphereCollider => SphereCollider - the sphere query now only returns a list of faces (extra info discarded). On the other hand it's a lot faster. - OBB, AABB and planes queries. Original OBB and AABB queries contributed by Erwin de Vries. - cosmetic changes in OPC_BoxBoxOverlap.h contributed by Gottfried Chen - some inlining problems fixed - faster ray-mesh tests using the separating axis theorem - new split value in AABB tree construction (contributed by Igor Kravtchenko). Provides faster queries most of the time. - improved temporal coherence for sphere & AABB queries (works in "All contacts" mode) Notes: - Everything in the "Ice code" directory (in VC++) is basically copy-pasted from my engine, with a lot of code removed until there was no link error anymore. Don't expect those files to be cute or anything, they've never been meant to be released and they're often updated/modified/messy. - Some experimental features have been removed as well. Else I would never have released the 1.2... - Not as polished/optimal as I would like it to be, but that's life. I promised myself to release it before october 2002 (one YEAR later ?!).... That's the only reason why it's there. - Some people reported ColDet was faster. Uh, come on. They were using Opcode in "All contacts" mode whereas ColDet was doing "first contact"... OPCODE distribution 1.1 (october 2001) ----------------------- New in Opcode 1.1: - stabbing queries - sphere queries - abtract base class for colliders - settings validation methods - compilation flags now grouped in OPC_Settings.h - smaller files, new VC++ virtual dirs (cleaner) Notes: - "override(baseclass)" is a personal cosmetic thing. It's the same as "virtual", but provides more info. - I code in 1600*1200, so some lines may look a bit long.. - This version is not as polished as the previous one due to lack of time. The stabbing & sphere queries can still be optimized: for example by trying other atomic overlap tests. I'm using my first ray-AABB code, but the newer one seems better. Tim Schröder's one is good as well. See: www.codercorner.com/RayAABB.cpp - The trees can easily be compressed even more, I save this for later (lack of time, lack of time!) - I removed various tests before releasing this one: - a separation line, a.k.a. "front" in QuickCD, because gains were unclear - distance queries in a PQP style, because it was way too slow - support for deformable models, too slow as well - You can easily use Opcode to do your player-vs-world collision detection, in a Nettle/Telemachos way. If someone out there wants to donate some art / level for the cause, I'd be glad to release a demo. (current demo uses copyrighted art I'm not allowed to spread) - Sorry for the lack of real docs and/or solid examples. I just don't have enough time. OPCODE distribution 1.0 (march 2001) ----------------------- - First release =============================================================================== WHAT ? OPCODE means OPtimized COllision DEtection. So this is a collision detection package similar to RAPID. Here's a quick list of features: - C++ interface, developed for Windows systems using VC++ 6.0 - Works on arbitrary meshes (convex or non-convex), even polygon soups - Current implementation uses AABB-trees - Introduces Primitive-BV overlap tests during recursive collision queries (whereas standard libraries only rely on Primitive-Primitive and BV-BV tests) - Introduces no-leaf trees, i.e. collision trees whose leaf nodes have been removed - Supports collision queries on quantized trees (decompressed on-the-fly) - Supports "first contact" or "all contacts" modes (à la RAPID) - Uses temporal coherence for "first contact" mode (~10 to 20 times faster, useful in rigid body simulation during bisection) - Memory footprint is 7.2 times smaller than RAPID's one, which is ideal for console games with limited ram (actually, if you use the unmodified RAPID code using double precision, it's more like 13 times smaller...) - And yet it often runs faster than RAPID (according to RDTSC, sometimes more than 5 times faster when objects are deeply overlapping) - Performance is usually close to RAPID's one in close-proximity situations - Stabbing, planes & volume queries (sphere, AABB, OBB, LSS) - Sweep-and-prune - Now works with deformable meshes - Hybrid trees What it can be used for: - standard mesh-mesh collision detection (similar to RAPID, SOLID, QuickCD, PQP, ColDet...) - N-body collisions (similar to V-Collide) - camera-vs-world collisions (similar to Telemachos/Paul Nettle/Stan Melax articles) - shadow feelers to speed up lightmap computations - in-out tests to speed up voxelization processes - picking - rigid body simulation - view frustum culling - etc WHY ? - Because RAPID uses too many bytes. - Because the idea was nice... WHEN ? It's been coded in march 2001 following a thread on the GD-Algorithms list. GDAlgorithms-list mailing list GDAlgorithms-list@lists.sourceforge.net http://lists.sourceforge.net/lists/listinfo/gdalgorithms-list WHO ? Pierre Terdiman June, 1, 2003 p.terdiman@wanadoo.fr p.terdiman@codercorner.com http://www.codercorner.com http://www.codercorner.com/Opcode.htm choreonoid-1.1.0+dfsg/src/Collision/Opcode/StdAfx.cpp000066400000000000000000000011021207742442300224100ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //#define ICE_MAIN #include "Stdafx.h" choreonoid-1.1.0+dfsg/src/Collision/Opcode/Stdafx.h000066400000000000000000000021401207742442300221200ustar00rootroot00000000000000/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * OPCODE - Optimized Collision Detection * Copyright (C) 2001 Pierre Terdiman * Homepage: http://www.codercorner.com/Opcode.htm */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// #if !defined(AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_) #define AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_ #ifndef __GNUC__ #if (_MSC_VER > 1000) #pragma once #endif // _MSC_VER > 1000 #endif // __GNU__ // Insert your headers here #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers #include "Opcode.h" //{{AFX_INSERT_LOCATION}} // Microsoft Visual C++ will insert additional declarations immediately before the previous line. #endif // !defined(AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_) choreonoid-1.1.0+dfsg/src/Collision/SSVTreeCollider.cpp000066400000000000000000000204111207742442300227630ustar00rootroot00000000000000 #include #include "SSVTreeCollider.h" #include "DistFuncs.h" static bool debug = false; SSVTreeCollider::SSVTreeCollider() { } bool SSVTreeCollider::Distance (BVTCache& cache, float& minD, Point &point0, Point&point1, const Matrix4x4* world0, const Matrix4x4* world1) { // Checkings if(!cache.Model0 || !cache.Model1) return false; if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false; if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false; // Checkings if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false; // Simple double-dispatch const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree(); const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree(); Distance(T0, T1, world0, world1, &cache, minD, point0, point1); return true; } void SSVTreeCollider::Distance (const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache, float& minD, Point &point0, Point&point1) { if (debug) std::cout << "Distance()" << std::endl; // Init collision query InitQuery(world0, world1); // Compute initial value using temporal coherency // todo : cache should be used const AABBCollisionNode *n; for (unsigned int i=0; iGetNbNodes(); i++){ n = tree0->GetNodes()+i; if (n->IsLeaf()){ mId0 = n->GetPrimitive(); break; } } for (unsigned int i=0; iGetNbNodes(); i++){ n = tree1->GetNodes()+i; if (n->IsLeaf()){ mId1 = n->GetPrimitive(); break; } } Point p0, p1; minD = PrimDist(mId0, mId1, p0, p1); // Perform distance computation _Distance(tree0->GetNodes(), tree1->GetNodes(), minD, p0, p1); // transform points TransformPoint4x3(point0, p0, *world1); TransformPoint4x3(point1, p1, *world1); // update cache cache->id0 = mId0; cache->id1 = mId1; } bool SSVTreeCollider::Collide (BVTCache& cache, double tolerance, const Matrix4x4* world0, const Matrix4x4* world1) { // Checkings if(!cache.Model0 || !cache.Model1) return false; if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false; if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false; // Checkings if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false; // Simple double-dispatch const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree(); const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree(); return Collide(T0, T1, world0, world1, &cache, tolerance); } bool SSVTreeCollider::Collide (const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache, double tolerance) { // Init collision query InitQuery(world0, world1); // todo : cache should be used // Perform collision detection if (_Collide(tree0->GetNodes(), tree1->GetNodes(), tolerance)){ // update cache cache->id0 = mId0; cache->id1 = mId1; return true; } return false; } float SSVTreeCollider::SsvSsvDist (const AABBCollisionNode *b0, const AABBCollisionNode *b1) { float dist = 0.0; CollisionAABB::ssv_type t1, t2; t1 = b0->mAABB.mType; t2 = b1->mAABB.mType; if(t1 == CollisionAABB::SSV_PSS && t2 == CollisionAABB::SSV_PSS){ dist = PssPssDist(b0->mAABB.mRadius, b0->mAABB.mCenter, b1->mAABB.mRadius, b1->mAABB.mCenter); } else if(t1 == CollisionAABB::SSV_PSS && t2 == CollisionAABB::SSV_LSS){ dist = PssLssDist(b0->mAABB.mRadius, b0->mAABB.mCenter, b1->mAABB.mRadius, b1->mAABB.mPoint0, b1->mAABB.mPoint1); } else if(t1 == CollisionAABB::SSV_LSS && t2 == CollisionAABB::SSV_PSS){ dist = LssPssDist(b0->mAABB.mRadius, b0->mAABB.mPoint0, b0->mAABB.mPoint1, b1->mAABB.mRadius, b1->mAABB.mCenter); } else if(t1 == CollisionAABB::SSV_LSS && t2 == CollisionAABB::SSV_LSS){ dist = PssPssDist(sqrtf(b0->GetSize()), b0->mAABB.mCenter, sqrtf(b1->GetSize()), b1->mAABB.mCenter); } else { throw "this ssv combination is not supported"; } return dist; } float SSVTreeCollider::PssPssDist (float r0, const Point& center0, float r1, const Point& center1) { Point c0; TransformPoint(c0, center0, mR0to1, mT0to1); return (center1-c0).Magnitude() - r0 - r1; } float SSVTreeCollider::PssLssDist (float r0, const Point& center0, float r1, const Point& point0, const Point& point1) { Point c0; TransformPoint(c0, center0, mR0to1, mT0to1); float d = PointSegDist(c0, point0, point1); return d - r0 - r1; } float SSVTreeCollider::LssPssDist (float r0, const Point& point0, const Point& point1, float r1, const Point& center0) { Point p0, p1; TransformPoint(p0, point0, mR0to1, mT0to1); TransformPoint(p1, point1, mR0to1, mT0to1); float d = PointSegDist(center0, p0, p1); return d - r0 - r1; } float SSVTreeCollider::LssLssDist (float r0, const Point& point0, const Point& point1, float r1, const Point& point2, const Point& point3) { Point p0, p1; TransformPoint(p0, point0, mR0to1, mT0to1); TransformPoint(p1, point1, mR0to1, mT0to1); float d = SegSegDist(p0, p1, point2, point3); return d - r0 - r1; } void SSVTreeCollider::_Distance (const AABBCollisionNode* b0, const AABBCollisionNode* b1, float& minD, Point& point0, Point& point1) { if (debug) std::cout << "_Distance()" << std::endl; mNowNode0 = b0; mNowNode1 = b1; float d; // Perform BV-BV distance test d = SsvSsvDist(b0, b1); if(d > minD) return; if(b0->IsLeaf() && b1->IsLeaf()) { Point p0, p1; d = PrimDist(b0->GetPrimitive(), b1->GetPrimitive(), p0, p1); if (d < minD){ minD = d; point0 = p0; point1 = p1; mId0 = b0->GetPrimitive(); mId1 = b1->GetPrimitive(); } return; } if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) { _Distance(b0->GetNeg(), b1, minD, point0, point1); _Distance(b0->GetPos(), b1, minD, point0, point1); } else { _Distance(b0, b1->GetNeg(), minD, point0, point1); _Distance(b0, b1->GetPos(), minD, point0, point1); } } bool SSVTreeCollider::_Collide (const AABBCollisionNode* b0, const AABBCollisionNode* b1, double tolerance) { mNowNode0 = b0; mNowNode1 = b1; float d; // Perform BV-BV distance test d = SsvSsvDist(b0, b1); if(d > tolerance) return false; if(b0->IsLeaf() && b1->IsLeaf()) { Point p0, p1; d = PrimDist(b0->GetPrimitive(), b1->GetPrimitive(), p0, p1); if (d <= tolerance){ mId0 = b0->GetPrimitive(); mId1 = b1->GetPrimitive(); return true; }else{ return false; } } if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) { if (_Collide(b0->GetNeg(), b1, tolerance)) return true; if (_Collide(b0->GetPos(), b1, tolerance)) return true; } else { if (_Collide(b0, b1->GetNeg(), tolerance)) return true; if (_Collide(b0, b1->GetPos(), tolerance)) return true; } return false; } float SSVTreeCollider::PrimDist(udword id0, udword id1, Point& point0, Point& point1) { // Request vertices from the app VertexPointers VP0; VertexPointers VP1; mIMesh0->GetTriangle(VP0, id0); mIMesh1->GetTriangle(VP1, id1); // Modified by S-cubed, Inc. // Transform from space 0 (old : 1) to space 1 (old : 0) // CD ã§ã¯å¤‰æ›ãŒé€†ãªã®ã§ã‚ã‚ã›ã‚‹ã€‚ Point u0,u1,u2; TransformPoint(u0, *VP0.Vertex[0], mR0to1, mT0to1); TransformPoint(u1, *VP0.Vertex[1], mR0to1, mT0to1); TransformPoint(u2, *VP0.Vertex[2], mR0to1, mT0to1); // Perform triangle-triangle distance test return TriTriDist(u0, u1, u2, *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2], point0, point1); } choreonoid-1.1.0+dfsg/src/Collision/SSVTreeCollider.h000066400000000000000000000113741207742442300224400ustar00rootroot00000000000000#ifndef __SSV_TREE_COLLIDER_H__ #define __SSV_TREE_COLLIDER_H__ #include "Opcode/Opcode.h" using namespace Opcode; /** * @brief collision detector based on SSV(Sphere Swept Volume) */ class SSVTreeCollider : public AABBTreeCollider { public: /** * @brief constructor */ SSVTreeCollider(); /** * @brief destructor */ ~SSVTreeCollider(){}; /** * @brief compute the minimum distance and the closest points * @param cache * @param minD the minimum distance * @param point0 the closest point on the first link * @param point1 the closest point on the second link * @param world0 transformation of the first link * @param world1 transformation of the second link * @return true if computed successfully, false otherwise */ bool Distance(BVTCache& cache, float& minD, Point &point0, Point&point1, const Matrix4x4* world0=null, const Matrix4x4* world1=null); /** * @brief detect collision between links. * @param cache * @param tolerance If distance between links is smaller than this value, it is regarded as collision * @param world0 transformation of the first link * @param world1 transformation of the second link * @return true if collision is detected, false otherwise */ bool Collide(BVTCache& cache, double tolerance, const Matrix4x4* world0=null, const Matrix4x4* world1=null); private: void Distance(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache, float& minD, Point &point0, Point&point1); void _Distance(const AABBCollisionNode* b0, const AABBCollisionNode* b1, float& minD, Point& point0, Point& point1); bool Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache, double tolerance); bool _Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1, double tolerance); /** * @brief compute distance between SSV(Swept Sphere Volume)s * @param b0 collision node from the left tree * @param b1 collision node from the right tree * @param return distance */ float SsvSsvDist(const AABBCollisionNode* b0, const AABBCollisionNode *b1); /** * @brief compute distance between primitives(triangles) * @param id0 index of the first primitive * @param id1 index of the second primitive * @param point0 the closest point on the first primitive * @param point1 the closest point on the second primitive * @return the minimum distance */ float PrimDist(udword id0, udword id1, Point& point0, Point& point1); /** * @brief compute distance between PSS(Point Swept Sphere) * @param r0 radius of the first sphere * @param center0 center of the first sphere * @param r1 radius of the first sphere * @param center1 center of the first sphere * @return distance */ float PssPssDist(float r0, const Point& center0, float r1, const Point& center1); /** * @brief compute distance between PSS(Point Swept Sphere) and LSS(Line Swept Sphere) * @param r0 radius of the PSS * @param center0 center of the PSS * @param r1 radius of the LSS * @param point0 one of end points of the line segment * @param point1 the other end points of the line segment * @return distance */ float PssLssDist(float r0, const Point& center0, float r1, const Point& point0, const Point& point1); /** * @brief compute distance between PSS(Point Swept Sphere) and LSS(Line Swept Sphere) * @param r0 radius of the PSS * @param point0 one of end points of the line segment * @param point1 the other end points of the line segment * @param r1 radius of the LSS * @param center0 center of the PSS * @return distance */ float LssPssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& center0); /** * @brief compute distance between LSS(Line Swept Sphere)s * @param r0 radius of the first LSS * @param point0 one of end points of the first line segment * @param point1 the other end points of the first line segment * @param r1 radius of the second LSS * @param point2 one of end points of the second line segment * @param point3 the other end points of the second line segment * @return distance */ float LssLssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& point2, const Point& point3); }; #endif choreonoid-1.1.0+dfsg/src/Collision/TriOverlap.cpp000066400000000000000000000767621207742442300221250ustar00rootroot00000000000000// // TriOverlap.cpp // #include "CollisionPairInserter.h" #include #include #include using namespace std; using namespace cnoid; namespace { const bool HIRUKAWA_DEBUG = false; /* used in normal_test */ enum { NOT_INTERSECT = 0, EDGE1_NOT_INTERSECT = 1, EDGE2_NOT_INTERSECT = 2, EDGE3_NOT_INTERSECT = 3 }; /* used in cross_test */ const int INTERSECT = 1; } /********************************************************** separability test by the supporting plane of a triangle return value 0 : not intersect 1 : f1 or e1 doesn't intersect 2 : f2 or e2 doesn't intersect 3 : f3 or e3 doesn't intersect **********************************************************/ static int separability_test_by_face(const Vector3& nm) { if(nm[0] < 0.0 && nm[1] < 0.0 && nm[2] < 0.0 || nm[0] > 0.0 && nm[1] > 0.0 && nm[2] > 0.0){ return NOT_INTERSECT; } if(nm[0] < 0.0 && nm[1] < 0.0 && nm[2] > 0.0 || nm[0] > 0.0 && nm[1] > 0.0 && nm[2] < 0.0){ return EDGE1_NOT_INTERSECT; } if(nm[0] < 0.0 && nm[1] > 0.0 && nm[2] > 0.0 || nm[0] > 0.0 && nm[1] < 0.0 && nm[2] < 0.0){ return EDGE2_NOT_INTERSECT; } if(nm[0] > 0.0 && nm[1] < 0.0 && nm[2] > 0.0 || nm[0] < 0.0 && nm[1] > 0.0 && nm[2] < 0.0){ return EDGE3_NOT_INTERSECT; } return 0; } /********************************************************** triangle inside test: normal vector is cross product of ei*fj ***********************************************************/ static int triangle_inside_test( const Vector3& ef1, const Vector3& ef2, const Vector3& ef3, const Vector3& P3, const Vector3& P1, const Vector3& P2, const Vector3& Q) { double ef1P1 = ef1.dot(P1); /*project P1 on ef1*/ double ef1P3 = ef1.dot(P3); /*project P3 on ef1*/ double ef1Q = ef1.dot(Q); /*project Q on ef1*/ double ef2P2 = ef2.dot(P2); /*project P2 on ef2*/ double ef2P1 = ef2.dot(P1); /*project P1 on ef2*/ double ef2Q = ef2.dot(Q); /*project Q on ef2*/ double ef3P3 = ef3.dot(P3); /*project P3 on ef3*/ double ef3P2 = ef3.dot(P2); /*project P2 on ef3*/ double ef3Q = ef3.dot(Q); /*project Q on ef3*/ if((ef1P3 > ef1P1 && ef1Q > ef1P1 || ef1P3 < ef1P1 && ef1Q < ef1P1 ) && (ef2P1 > ef2P2 && ef2Q > ef2P2 || ef2P1 < ef2P2 && ef2Q < ef2P2 ) && (ef3P2 > ef3P3 && ef3Q > ef3P3 || ef3P2 < ef3P3 && ef3Q < ef3P3 )) { return INTERSECT; } return NOT_INTERSECT; } static void find_intersection_pt( Vector3& ipt, const Vector3& x1, const Vector3& x2, double mn1, double mn2) { if(mn1 == mn2) /*exit(0);*/ return; if(mn1 >0 && mn2 < 0){ ipt = (-(mn2*x1) + mn1*x2)/(mn1-mn2); }else if(mn1 < 0 && mn2 > 0){ ipt = (mn2*x1 - mn1*x2)/(-mn1+mn2); } } static inline void find_intersection_pt( Vector3& ipt, const Vector3& x1, const Vector3& x2, double p) { ipt = (1.0 - p) * x1 + p * x2; if(HIRUKAWA_DEBUG){ cout << "v1 = " << x1[0] << ", " << x1[1] << ", " << x1[2] << endl; cout << "v2 = " << x2[0] << ", " << x2[1] << ", " << x2[2] << endl; cout << "edge = " << x2[0]-x1[0] << ", " << x2[1]-x1[1] << ", " << x2[2]-x1[2] << endl; cout << "common pt = " << ipt[0] << " " << ipt[1] << " " << ipt[2] << endl; } } // // Calculate the depth of the intersection between two triangles // static inline double calc_depth( const Vector3& ip1, const Vector3& ip2, const Vector3& n) { // vecNormalize(n); if(HIRUKAWA_DEBUG){ cout << "calc_depth 1 = " << (ip1 - ip2).dot(n) << endl; } return fabs((ip1 - ip2).dot(n)); } static double calc_depth( const Vector3& ip, const Vector3& pt1, const Vector3& pt2, const Vector3& n) { double d1 = fabs((ip - pt1).dot(n)); double d2 = fabs((ip - pt2).dot(n)); double depth = (d1 < d2) ? d1 : d2; if(HIRUKAWA_DEBUG){ cout << "calc_depth 2 = " << depth << endl; } return depth; } static double calc_depth( const Vector3& ip1, const Vector3& ip2, const Vector3& pt1, const Vector3& pt2, const Vector3& pt3, const Vector3& n) { // when a triangle penetrates another triangle at two intersection points // and the separating plane is the supporting plane of the penetrated triangle // vecNormalize(n); double d1 = fabs((ip1 - pt1).dot(n)); double d2 = fabs((ip2 - pt2).dot(n)); double d3 = fabs((ip1 - pt3).dot(n)); // ip1 can be either ip1 or ip2 double depth = (d1 < d2) ? d2 : d1; if(d3 < depth){ depth = d3; } if(HIRUKAWA_DEBUG){ cout << "calc_depth 3 = " << depth << endl; } return depth; } static void find_foot( const Vector3& ip, const Vector3& pt1, const Vector3& pt2, Vector3& f) { /* double u, v, w, p; u = pt2[0] - pt1[0]; v = pt2[1] - pt1[1]; w = pt2[2] - pt1[2]; p = u * (ip[0] - pt1[0]) + v * (ip[1] - pt1[1]) + w * (ip[2] - pt1[2]); p /= u * u + v * v + w * w; f[0] = pt1[0] + u * p; f[1] = pt1[1] + v * p; f[2] = pt1[2] + w * p; */ const Vector3 pt = pt2 - pt1; const double p = pt.dot(ip - pt1) / pt.dot(pt); f = pt1 + p * pt; } static double calc_depth( const Vector3& ip, const Vector3& pt1, const Vector3& pt2, const Vector3& pt3, const Vector3& n) { Vector3 f12, f23, f31; find_foot(ip, pt1, pt2, f12); find_foot(ip, pt2, pt3, f23); find_foot(ip, pt3, pt1, f31); if(HIRUKAWA_DEBUG){ cout << "ip = " << ip[0] << " " << ip[1] << " " << ip[2] << endl; cout << "f12 = " << f12[0] << " " << f12[1] << " " << f12[2] << endl; cout << "f23 = " << f23[0] << " " << f23[1] << " " << f23[2] << endl; cout << "f31 = " << f31[0] << " " << f31[1] << " " << f31[2] << endl; } // fabs() is taken to cope with numerical error of find_foot() const double d1 = fabs((f12 - ip).dot(n)); const double d2 = fabs((f23 - ip).dot(n)); const double d3 = fabs((f31 - ip).dot(n)); // cout << "d1 d2 d3 = " << d1 << " " << d2 << " " << d3 << endl; // dsum = fabs(d1)+fabs(d2)+fabs(d3); // if(d1<0.0) d1=dsum; if(d2<0.0) d2=dsum; if(d3<0.0) d3=dsum; double depth = (d1 < d2) ? d1 : d2; if(d3 < depth){ depth = d3; } if(HIRUKAWA_DEBUG){ cout << "calc_depth 4 = " << depth << endl; } return depth; } static double calc_depth2( const Vector3& ip1, const Vector3& ip2, const Vector3& pt1, const Vector3& pt2, const Vector3& pt3, const Vector3& n) { // when a triangle penetrates another triangle at two intersecting points // and the separating plane is the supporting plane of the penetrating triangle const Vector3 nn(n); // vecNormalize(nn); const double depth1 = calc_depth(ip1, pt1, pt2, pt3, nn); const double depth2 = calc_depth(ip2, pt1, pt2, pt3, nn); // cout << "depth1 depth2 = " << depth1 << " " << depth2 << endl; const double depth = (depth1 < depth2) ? depth2 : depth1; if(HIRUKAWA_DEBUG){ cout << "calc_depth 5 = " << depth << endl; } return depth; } static void calcNormal( Vector3& vec, const Vector3& v1, const Vector3& v2, const Vector3& v3, double sgn) { // find the vector from v1 to the mid point of v2 and v3 when 0n_vector = m1 * pq; // // The depth is estimated in InsertCollisionPair.cpp // The following depth calculation is done only for debugging purpose // col_p->depth = calc_depth(ip1, ip2, p2, p3, p1, col_p->n_vector); const Vector3 nv = -n1 * pq; const double dp = calc_depth2(ip1, ip2, q1, q2, q3, nv); if(dp < col_p->depth){ col_p->depth = dp; } ip3 = ip1; ip4 = ip2; col_p->num_of_i_points = 2; return 1; } return 0; } // // find the collision info when an edges penetrate a face each other // static int find_collision_info( const Vector3& p1, const Vector3& p2, const Vector3& p3, double mp0, double mp1, const Vector3& q1, const Vector3& q2, const Vector3& q3, double nq0, double nq1, const Vector3& ef11, const Vector3& n1, const Vector3& m1, Vector3& ip3, Vector3& ip4, collision_data *col_p) { Vector3 ip1; find_intersection_pt(ip1, q1, q2, nq0, nq1); Vector3 ip2; find_intersection_pt(ip2, p1, p2, mp0, mp1); double dp; if(get_normal_vector_test(col_p->n_vector, ef11, ip2, ip1, p3, q3, n1, m1, p1, p2, q1, q2) && find_common_perpendicular(p1, p2, q1, q2, ip1, ip2, n1, m1, col_p->n_vector, dp)){ ip3 = ip1; ip4 = ip2; col_p->num_of_i_points = 2; col_p->depth = dp; return 1; } return 0; } // very robust triangle intersection test // uses no divisions // works on coplanar triangles int tri_tri_overlap( const Vector3& P1, const Vector3& P2, const Vector3& P3, const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, collision_data* col_p, CollisionPairInserter* collisionPairInserter) { /* One triangle is (p1,p2,p3). Other is (q1,q2,q3). Edges are (e1,e2,e3) and (f1,f2,f3). Normals are n1 and m1 Outwards are (g1,g2,g3) and (h1,h2,h3). We assume that the triangle vertices are in the same coordinate system. First thing we do is establish a new c.s. so that p1 is at (0,0,0). */ Vector3 p1, p2, p3; Vector3 q1, q2, q3; Vector3 e1, e2, e3; Vector3 f1, f2, f3; // Vector3 g1, g2, g3; // Vector3 h1, h2, h3; Vector3 n1, m1; Vector3 z; Vector3 nq, mp; int triP,triQ; int edf1, edf2, edf3, ede1, ede2, ede3; Vector3 ef11, ef12, ef13; Vector3 ef21, ef22, ef23; Vector3 ef31, ef32, ef33; /* intersection point R is a flag which tri P & Q correspond or not */ Vector3 ip,ip3,ip4,ip5,ip6; Vector3 i_pts_w[4]; const int FV = 1; // face-vertex contact type const int VF = 2; // vertex-face contact type const int EE = 3; // edge-edge contact type z.setZero(); p1 = P1 - P1; p2 = P2 - P1; p3 = P3 - P1; q1 = Q1 - P1; q2 = Q2 - P1; q3 = Q3 - P1; e1 = p2 - p1; e2 = p3 - p2; e3 = p1 - p3; f1 = q2 - q1; f2 = q3 - q2; f3 = q1 - q3; n1 = e1.cross(e2); m1 = f1.cross(f2); // now begin the series of tests /************************************* separability test by face ************************************/ nq[0] = n1.dot(q1); nq[1] = n1.dot(q2); nq[2] = n1.dot(q3); triQ = separability_test_by_face(nq); if(triQ == NOT_INTERSECT) return 0; double mq = m1.dot(q1); mp[0] = m1.dot(p1) - mq; mp[1] = m1.dot(p2) - mq; mp[2] = m1.dot(p3) - mq; triP = separability_test_by_face(mp); if(triP == NOT_INTERSECT) return 0; ef11 = e1.cross(f1); ef12 = e1.cross(f2); ef13 = e1.cross(f3); ef21 = e2.cross(f1); ef22 = e2.cross(f2); ef23 = e2.cross(f3); ef31 = e3.cross(f1); ef32 = e3.cross(f2); ef33 = e3.cross(f3); edf1 = 0; edf2 = 0; edf3 = 0; ede1 = 0; ede2 = 0; ede3 = 0; /******************************** triangle inside test *********************************/ switch(triQ) { case NOT_INTERSECT: return 0; case EDGE1_NOT_INTERSECT: edf2 = triangle_inside_test(ef12,ef22,ef32,p3,p1,p2,q2); edf3 = triangle_inside_test(ef13,ef23,ef33,p3,p1,p2,q3); break; case EDGE2_NOT_INTERSECT: edf1 = triangle_inside_test(ef11,ef21,ef31,p3,p1,p2,q1); edf3 = triangle_inside_test(ef13,ef23,ef33,p3,p1,p2,q3); break; case EDGE3_NOT_INTERSECT: edf1 = triangle_inside_test(ef11,ef21,ef31,p3,p1,p2,q1); edf2 = triangle_inside_test(ef12,ef22,ef32,p3,p1,p2,q2); break; } int num_of_edges = edf1 + edf2 + edf3; if(num_of_edges == 3){ //exit(1); return 0; } if(num_of_edges < 2){ switch(triP) { case EDGE1_NOT_INTERSECT: ede2 = triangle_inside_test(ef21, ef22, ef23, q3, q1, q2, p2); ede3 = triangle_inside_test(ef31, ef32, ef33, q3, q1, q2, p3); if(ede2 + ede3 == 2){ edf1 = NOT_INTERSECT; edf2 = NOT_INTERSECT; edf3 = NOT_INTERSECT; } break; case EDGE2_NOT_INTERSECT: ede1 = triangle_inside_test(ef11, ef12, ef13, q3, q1, q2, p1); ede3 = triangle_inside_test(ef31, ef32, ef33, q3, q1, q2, p3); if(ede1 + ede3 == 2){ edf1 = NOT_INTERSECT; edf2 = NOT_INTERSECT; edf3 = NOT_INTERSECT; } break; case EDGE3_NOT_INTERSECT: ede1 = triangle_inside_test(ef11, ef12, ef13, q3, q1, q2, p1); ede2 = triangle_inside_test(ef21, ef22, ef23, q3, q1, q2, p2); if(ede1 + ede2 == 2){ edf1 = NOT_INTERSECT; edf2 = NOT_INTERSECT; edf3 = NOT_INTERSECT; } break; } if(num_of_edges == 0 && (ede1 + ede2 + ede3) == 3){ //exit(1); return 0; } } int num = edf1 + edf2 + edf3 + ede1 + ede2 + ede3; if(num == 0){ // cout << "no edge intersect" << endl; return 0; } else if(num > 2){ printf("err of edge detection...."); //exit(1); return 0; } n1.normalize(); m1.normalize(); /********************************* find intersection points **********************************/ if(num == 1){ if(edf1 == INTERSECT){ find_intersection_pt(ip, q1, q2, nq[0], nq[1]); ip3 = ip; col_p->n_vector = -n1; col_p->depth = 0.0; col_p->c_type = FV; } else if(edf2 == INTERSECT){ find_intersection_pt(ip, q2, q3, nq[1], nq[2]); ip3 = ip; col_p->n_vector = -n1; col_p->depth = 0.0; col_p->c_type = FV; } else if(edf3 == INTERSECT){ find_intersection_pt(ip, q3, q1, nq[2], nq[0]); ip3 = ip; col_p->n_vector = -n1; col_p->depth = 0.0; col_p->c_type = FV; } else if(ede1 == INTERSECT){ find_intersection_pt(ip, p1, p2, mp[0], mp[1]); ip3 = ip; col_p->n_vector = m1; col_p->depth = 0.0; col_p->c_type = VF; } else if(ede2 == INTERSECT){ find_intersection_pt(ip, p2, p3, mp[1], mp[2]); ip3 = ip; col_p->n_vector = m1; col_p->depth = 0.0; col_p->c_type = VF; } else if(ede3 == INTERSECT){ find_intersection_pt(ip, p3, p1, mp[2], mp[0]); ip3 = ip; col_p->n_vector = m1; col_p->depth = 0.0; col_p->c_type = VF; } col_p->num_of_i_points = 1; } else if(num == 2) { if(edf1 == INTERSECT && edf2 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "f1 f2" << endl; col_p->c_type = FV; if(!find_collision_info(q2,q1,q3,nq[1],nq[0],nq[2],p1,p2,p3,e1,e2,e3, m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0)){ return 0; } } else if(edf1 == INTERSECT && edf3 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "f1 f3" << endl; col_p->c_type = FV; if(!find_collision_info(q1,q2,q3,nq[0],nq[1],nq[2],p1,p2,p3,e1,e2,e3, m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0)){ return 0; } } else if(ede1 == INTERSECT && edf1 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e1 f1" << endl; col_p->c_type = EE; if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q1,q2,q3,nq[0],nq[1],ef11, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(ede2 == INTERSECT && edf1 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e2 f1" << endl; col_p->c_type = EE; if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q1,q2,q3,nq[0],nq[1],ef21, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(ede3 == INTERSECT && edf1 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e3 f1" << endl; col_p->c_type = EE; if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q1,q2,q3,nq[0],nq[1],ef31, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(edf2 == INTERSECT && edf3 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "f2 f3" << endl; col_p->c_type = FV; if(!find_collision_info(q3,q2,q1,nq[2],nq[1],nq[0],p1,p2,p3,e1,e2,e3, m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0)){ return 0; } } else if(ede1 == INTERSECT && edf2 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e1 f2" << endl; col_p->c_type = EE; if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q2,q3,q1,nq[1],nq[2],ef12, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(ede2 == INTERSECT && edf2 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e2 f2" << endl; col_p->c_type = EE; if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q2,q3,q1,nq[1],nq[2],ef22, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(ede3 == INTERSECT && edf2 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e3 f2" << endl; col_p->c_type = EE; if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q2,q3,q1,nq[1],nq[2],ef32, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(ede1 == INTERSECT && edf3 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e1 f3" << endl; col_p->c_type = EE; if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q3,q1,q2,nq[2],nq[0],ef13, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(ede2 == INTERSECT && edf3 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e2 f3" << endl; col_p->c_type = EE; if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q3,q1,q2,nq[2],nq[0],ef23, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(ede3 == INTERSECT && edf3 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e3 f3" << endl; col_p->c_type = EE; if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q3,q1,q2,nq[2],nq[0],ef33, n1,m1,ip3,ip4,col_p)){ return 0; } } else if(ede1 == INTERSECT && ede2 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e1 e2" << endl; col_p->c_type = VF; if(!find_collision_info(p2,p1,p3,mp[1],mp[0],mp[2],q1,q2,q3,f1,f2,f3, n1,m1,ip3,ip4,ip5,ip6,col_p,1.0)){ return 0; } } else if(ede1 == INTERSECT && ede3 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e1 e3" << endl; col_p->c_type = VF; if(!find_collision_info(p1,p2,p3,mp[0],mp[1],mp[2],q1,q2,q3,f1,f2,f3, n1,m1,ip3,ip4,ip5,ip6,col_p,1.0)){ return 0; } } else if(ede2 == INTERSECT && ede3 == INTERSECT){ if(HIRUKAWA_DEBUG) cout << "e2 e3" << endl; col_p->c_type = VF; if(!find_collision_info(p3,p2,p1,mp[2],mp[1],mp[0],q1,q2,q3,f1,f2,f3, n1,m1,ip3,ip4,ip5,ip6,col_p,1.0)){ return 0; } } } if(col_p->num_of_i_points == 1){ col_p->i_points[0] = ip3 + P1; } else if(col_p->num_of_i_points == 2){ col_p->i_points[0] = ip3 + P1; col_p->i_points[1] = ip4 + P1; } else if(col_p->num_of_i_points == 3){ col_p->i_points[0] = ip3 + P1; col_p->i_points[1] = ip4 + P1; col_p->i_points[2] = ip5 + P1; } else if(col_p->num_of_i_points == 4){ col_p->i_points[0] = ip3 + P1; col_p->i_points[1] = ip4 + P1; col_p->i_points[2] = ip5 + P1; col_p->i_points[3] = ip5 + P1; } col_p->n = n1; col_p->m = m1; if(HIRUKAWA_DEBUG){ CollisionPairInserter& c = *collisionPairInserter; Vector3 p1w = c.CD_s2 * (c.CD_Rot2 * P1 + c.CD_Trans2); Vector3 p2w = c.CD_s2 * (c.CD_Rot2 * P2 + c.CD_Trans2); Vector3 p3w = c.CD_s2 * (c.CD_Rot2 * P3 + c.CD_Trans2); Vector3 q1w = c.CD_s2 * (c.CD_Rot2 * Q1 + c.CD_Trans2); Vector3 q2w = c.CD_s2 * (c.CD_Rot2 * Q2 + c.CD_Trans2); Vector3 q3w = c.CD_s2 * (c.CD_Rot2 * Q3 + c.CD_Trans2); cout << "P1 = " << p1w[0] << " " << p1w[1] << " " << p1w[2] << endl; cout << "P2 = " << p2w[0] << " " << p2w[1] << " " << p2w[2] << endl; cout << "P3 = " << p3w[0] << " " << p3w[1] << " " << p3w[2] << endl; cout << "Q1 = " << q1w[0] << " " << q1w[1] << " " << q1w[2] << endl; cout << "Q2 = " << q2w[0] << " " << q2w[1] << " " << q2w[2] << endl; cout << "Q3 = " << q3w[0] << " " << q3w[1] << " " << q3w[2] << endl; for(int i=0; inum_of_i_points; i++){ i_pts_w[i] = c.CD_s2 * ((c.CD_Rot2 * col_p->i_points[i]) + c.CD_Trans2); cout << i << "-th intersecting point = "; cout << i_pts_w[i][0] << " " << i_pts_w[i][1] << " " << i_pts_w[i][2] << endl; } cout << "n1 = " << n1[0] << " " << n1[1] << " " << n1[2] << endl; cout << "m1 = " << m1[0] << " " << m1[1] << " " << m1[2] << endl; cout << "mp[0] mp[1] mp[2] = " << mp[0] << " " << mp[1] << " " << mp[2] << endl; cout << "nq[0] nq[1] nq[2] = " << nq[0] << " " << nq[1] << " " << nq[2] << endl; cout << "n_vector = " << col_p->n_vector[0] << " " << col_p->n_vector[1] << " " << col_p->n_vector[2] << endl; cout << "depth = " << col_p->depth << endl << endl;; } #if TRACE1 printf("intersect point...in tri_contact..\n"); printf(" ip1x = %f ip1y = %f ip1z = %f\n ip2x = %f ip2y = %f ip2z = %f\n", col_p->i_points[0][0],col_p->i_points[0][1],col_p->i_points[0][2], col_p->i_points[1][0],col_p->i_points[1][1],col_p->i_points[1][2]); printf("normal vector....it tri_conctact..\n"); printf("N[0] = %f,N[1] = %f,N[2] = %f\n",col_p->n_vector[0],col_p->n_vector[1],col_p->n_vector[2]); #endif return 1; } choreonoid-1.1.0+dfsg/src/Collision/exportdecl.h000066400000000000000000000004531207742442300216340ustar00rootroot00000000000000 #ifdef CNOID_EXPORT #undef CNOID_EXPORT #endif #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) # ifdef CnoidCollision_EXPORTS # define CNOID_EXPORT __declspec(dllexport) # else # define CNOID_EXPORT __declspec(dllimport) # endif #else # define CNOID_EXPORT #endif choreonoid-1.1.0+dfsg/src/GRobotPlugin/000077500000000000000000000000001207742442300177305ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/GRobotPlugin/CMakeLists.txt000066400000000000000000000025621207742442300224750ustar00rootroot00000000000000# @author Shin'ichiro Nakaoka option(BUILD_GROBOT_PLUGIN "Building GRobotPlugin" OFF) if(NOT BUILD_GROBOT_PLUGIN) return() endif() set(target CnoidGRobotPlugin) make_gettext_mofiles(mofiles) QT4_ADD_RESOURCES(RC_SRCS GRobotPlugin.qrc) add_library(${target} SHARED GRobotPlugin.cpp GRobotBar.cpp GRobotControllerItem.cpp GRobotController.cpp ${mofiles} ${RC_SRCS} ) if(UNIX) target_link_libraries(${target} CnoidBodyPlugin ${Boost_THREAD_LIBRARY} pthread rt) elseif(MSVC) target_link_libraries(${target} CnoidBodyPlugin ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY}) set_target_properties(${target} PROPERTIES COMPILE_DEFINITIONS "BOOST_ASIO_DISABLE_STD_ARRAY") endif() apply_common_setting_for_plugin(${target}) install_external_libraries(${Boost_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY}) # set(CMAKE_BUILD_TYPE Debug) set(customizer GRobotCustomizer) add_library(${customizer} SHARED GRobotCustomizer.cpp ${mofile}) set_target_properties(${customizer} PROPERTIES PREFIX "") set_target_properties(${customizer} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CNOID_PLUGIN_SUBDIR}/customizer) install(TARGETS ${customizer} RUNTIME DESTINATION ${CNOID_PLUGIN_SUBDIR}/customizer CONFIGURATIONS Release Debug LIBRARY DESTINATION ${CNOID_PLUGIN_SUBDIR}/customizer CONFIGURATIONS Release Debug) choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotBar.cpp000066400000000000000000000022231207742442300222540ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "GRobotBar.h" #include #include "gettext.h" using namespace boost; using namespace cnoid; GRobotBar* GRobotBar::instance() { static GRobotBar* instance = new GRobotBar(); return instance; } GRobotBar::GRobotBar() : ToolBar(N_("GRobotBar")) { addImage(":/GRobot/icons/grobo-logo.png") ->setToolTip(_("G-Robot toolbar which provides buttons for handling actual G-Robots")); addSpacing(4); addSeparator(); addToggleButton(QIcon(":/GRobot/icons/servo-on.png"), _("Turn on / off servo gains")) ->sigToggled().connect(bind(&GRobotBar::onServoButtonToggled, this, _1)); addButton(QIcon(":/GRobot/icons/sendpose.png"), _("Send the current pose of virtual robots to actual robots")) ->sigClicked().connect(bind(ref(sigPoseSendRequest_))); syncCheck = addToggleButton(QIcon(":/GRobot/icons/syncpose.png"), _("Synchronize the pose of actual robots pose with virtual robots")); syncCheck->sigToggled().connect(bind(ref(sigSyncModeToggled_), _1)); } void GRobotBar::onServoButtonToggled(bool on) { sigServoSwitchRequest_(on); } choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotBar.h000066400000000000000000000020501207742442300217170ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_GROBOT_GROBOT_BAR_H_INCLUDED #define CNOID_GROBOT_GROBOT_BAR_H_INCLUDED #include #include namespace cnoid { class GRobotBar : public ToolBar { public: static GRobotBar* instance(); SignalProxy< boost::signal > sigSyncModeToggled() { return sigSyncModeToggled_; } SignalProxy< boost::signal > sigServoSwitchRequest() { return sigServoSwitchRequest_; } SignalProxy< boost::signal > sigPoseSendRequest() { return sigPoseSendRequest_; } bool isSyncMode() { return syncCheck->isChecked(); } private: GRobotBar(); ToolButton* syncCheck; boost::signal sigSyncModeToggled_; boost::signal sigServoSwitchRequest_; boost::signal sigPoseSendRequest_; void onServoButtonToggled(bool on); }; } #endif choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotController.cpp000066400000000000000000000367731207742442300237140ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "GRobotController.h" #include #include #include #include using namespace std; using namespace boost; namespace { const bool MEASURE_TIMER_INTERVALS = true; #ifdef WIN32 const char* defaultPortDevice = "COM0"; #else const char* defaultPortDevice = "/dev/ttyUSB0"; #endif inline double degree(double rad) { return (180.0 * rad / M_PI); } const int NJOINTS = 20; const char jointIdToMotorIdMap[] = { 8 /* R_HIP_Y */, 10 /* R_HIP_R */, 9 /* R_HIP_P */, 11 /* R_KNEE_P */, 12 /* R_ANKLE_P */, 13 /* R_ANKLE_R */, 14 /* L_HIP_Y */, 16 /* L_HIP_R */, 15 /* L_HIP_P */, 17 /* L_KNEE_P */, 18 /* L_ANKLE_P */, 19 /* L_ANKLE_R */, 0 /* CHEST_P */, 1 /* NECK_Y */, 2 /* R_SHOULDER_P */, 3 /* R_SHOULDER_R */, 4 /* R_ELBOW_P */, 5 /* R_SHOULDER_P */, 6 /* R_SHOULDER_R */, 7 /* R_ELBOW_P */ }; const unsigned char PoseCommand[] = { 0x53 ,0x6c ,0xfa,0xaf ,0x00 ,0x00 ,0x1e ,0x05, 0x14, 0x01 ,0x00 ,0x00 ,0x64 ,0x00, 0x02 ,0x00 ,0x00 ,0x64 ,0x00, 0x03 ,0x00 ,0x00 ,0x64 ,0x00, 0x04 ,0x00 ,0x00 ,0x64 ,0x00, 0x05 ,0x00 ,0x00 ,0x64 ,0x00, 0x06 ,0x00 ,0x00 ,0x64 ,0x00, 0x07 ,0x00 ,0x00 ,0x64 ,0x00, 0x08 ,0x00 ,0x00 ,0x64 ,0x00, 0x09 ,0x00 ,0x00 ,0x64 ,0x00, 0x0a ,0x00 ,0x00 ,0x64 ,0x00, 0x0b ,0x00 ,0x00 ,0x64 ,0x00, 0x0c ,0x00 ,0x00 ,0x64 ,0x00, 0x0d ,0x00 ,0x00 ,0x64 ,0x00, 0x0e ,0x00 ,0x00 ,0x64 ,0x00, 0x0f ,0x00 ,0x00 ,0x64 ,0x00, 0x10 ,0x00 ,0x00 ,0x64 ,0x00, 0x11 ,0x00 ,0x00 ,0x64 ,0x00, 0x12 ,0x00 ,0x00 ,0x64 ,0x00, 0x13 ,0x00 ,0x00 ,0x64 ,0x00, 0x14 ,0x00 ,0x00 ,0x64 ,0x00, 0x1b }; } GRobotController::GRobotController() : portDevice_(defaultPortDevice), port(io), poseCommand(sizeof(PoseCommand)), tmpJointAngles(NJOINTS, 0.0), jointAngles(NJOINTS, 0.0) { init(); } GRobotController::GRobotController(const GRobotController& org) : portDevice_(org.portDevice_), port(io), poseCommand(sizeof(PoseCommand)), tmpJointAngles(NJOINTS, 0.0), jointAngles(NJOINTS, 0.0) { init(); } void GRobotController::init() { transitionTime = 0.1; memcpy(&poseCommand[0], PoseCommand, sizeof(PoseCommand)); mode = ON_DEMAND_POSE_SENDING; #ifndef WIN32 sem_init(&semTimer, 0, 0); #endif } GRobotController::~GRobotController() { poseSendingMutex.lock(); mode = EXIT_POSE_SENDING; poseSendingMutex.unlock(); poseSendingCondition.notify_all(); poseSendingThread.join(); closeSerialPort(); #ifndef WIN32 sem_destroy(&semTimer); #endif } int GRobotController::numJoints() const { return NJOINTS; } const std::string& GRobotController::portDevice() const { return portDevice_; } void GRobotController::setPortDevice(const std::string& device) { portDevice_ = device; } bool GRobotController::openSerialPort() { if(!port.is_open()){ boost::system::error_code error; port.open(portDevice_, error); if(!error){ port.set_option(asio::serial_port_base::baud_rate(115200)); port.set_option(asio::serial_port_base::character_size(8)); port.set_option(asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none)); port.set_option(asio::serial_port_base::parity(asio::serial_port_base::parity::none)); port.set_option(asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one)); return true; } } return port.is_open(); } void GRobotController::closeSerialPort() { if(port.is_open()){ boost::system::error_code error; port.close(error); } } bool GRobotController::sendData(unsigned char* data, int len) { boost::system::error_code error; asio::write(port, asio::buffer(data, len), boost::asio::transfer_all(), error); if(error){ closeSerialPort(); } return !error; } bool GRobotController::receiveData(char* buf, int len) { boost::system::error_code error; int n = 0; for(int i=0; i < 100; ++i){ int c = port.read_some(asio::buffer(buf, len), error); if(error){ break; } n += c; if(n == len){ break; } this_thread::sleep(posix_time::microseconds(100)); } return (n == len); } bool GRobotController::checkConnection() { static unsigned char command[] = { 0x41, 0x00 }; char buf; if(sendData(command, 2)){ this_thread::sleep(posix_time::milliseconds(10)); if(receiveData(&buf, 1) && buf == 0x07){ return true; } } return false; } bool GRobotController::makeConnection() { if(port.is_open()){ return true; } if(openSerialPort() && checkConnection()){ return true; } closeSerialPort(); return false; } char GRobotController::calcSum(unsigned char* data, int len) { char sum = 0; for(int i=0; i < len; ++i){ sum ^= data[i]; } return sum; } void GRobotController::switchServo(int jointId, bool on) { unsigned char command[] = { 0x53, 0x09, 0xfa, 0xaf, 0x01, 0x00, 0x24, 0x01, 0x01, 0x01, 0x26 }; command[4] = jointIdToMotorIdMap[jointId]; command[9] = on ? 1 : 0; command[10] = calcSum(&command[4], 6); sendData(command, 11); } void GRobotController::setJointAngleCommand(int jointId, double q, double ttime) { int motorId = jointIdToMotorIdMap[jointId]; unsigned char* command = &poseCommand[motorId * 5 + 10]; short qcom = static_cast(degree(q) * 10.0); command[0] = qcom & 0xff; command[1] = (qcom >> 8) & 0xff; unsigned short t = static_cast(ttime * 100.0); command[2] = t & 0xff; command[3] = (t >> 8) & 0xff; } #if WIN32 void GRobotController::initializeTimer() { timerQueue = CreateTimerQueue(); isTimerAvailable = (timerQueue != NULL); isTimerActive = false; } bool GRobotController::startTimer(double interval) { if(MEASURE_TIMER_INTERVALS){ QueryPerformanceFrequency(&pcfreq); QueryPerformanceCounter(&pc0); } if(isTimerAvailable){ int msecInterval = static_cast(floor(interval * 1000.0 + 0.5)); if(CreateTimerQueueTimer(&timerHandle, timerQueue, (WAITORTIMERCALLBACK)timerCallback, (PVOID)this, msecInterval, msecInterval, WT_EXECUTEINIOTHREAD)){ isTimerActive = true; } } return isTimerActive; } void GRobotController::stopTimer() { if(isTimerActive){ if(DeleteTimerQueueTimer(timerQueue, timerHandle, (HANDLE)-1)){ isTimerActive = false; } } } void GRobotController::finalizeTimer() { if(isTimerAvailable){ DeleteTimerQueueEx(timerQueue, (HANDLE)-1); timerQueue = NULL; } isTimerActive = false; isTimerAvailable = false; } VOID WINAPI GRobotController::timerCallback(PVOID lpParameter, BOOL TimerOrWaitFired) { reinterpret_cast(lpParameter)->onMotionFrameSendingRequest(); } #else void GRobotController::initializeTimer() { struct sigaction sa; sa.sa_flags = SA_SIGINFO | SA_RESTART; sa.sa_sigaction = timerHandler; sigemptyset(&sa.sa_mask); if(sigaction(SIGRTMIN, &sa, NULL) == -1){ return; } struct sigevent sev; sev.sigev_notify = SIGEV_SIGNAL; sev.sigev_signo = SIGRTMIN; sev.sigev_value.sival_ptr = this; sev.sigev_notify_attributes = 0; isTimerAvailable = (timer_create(CLOCK_MONOTONIC, &sev, &timerID) >= 0); isTimerActive = false; } bool GRobotController::startTimer(double interval) { if(MEASURE_TIMER_INTERVALS){ gettimeofday(&tv0, 0); } if(isTimerAvailable){ struct itimerspec tspec; tspec.it_interval.tv_sec = 0; tspec.it_interval.tv_nsec = floor(interval * 1.0e9 + 0.5); tspec.it_value.tv_sec = 0; tspec.it_value.tv_nsec = floor(interval * 1.0e9 + 0.5); isTimerActive = (timer_settime(timerID, 0, &tspec, 0) >= 0); } return isTimerActive; } void GRobotController::stopTimer() { if(isTimerAvailable){ struct itimerspec tspec; tspec.it_interval.tv_sec = 0; tspec.it_interval.tv_nsec = 0; tspec.it_value.tv_sec = 0; tspec.it_value.tv_nsec = 0; timer_settime(timerID, 0, &tspec, 0); isTimerActive = false; } } void GRobotController::finalizeTimer() { timer_delete(timerID); isTimerAvailable = false; } void GRobotController::timerHandler(int sig, siginfo_t* si, void* uc) { GRobotController* self = static_cast(si->si_value.sival_ptr); if(self){ sem_post(&self->semTimer); // wake up the continuos pose sending loop } } #endif void GRobotController::poseSendingLoop() { initializeTimer(); while(true){ poseSendingMutex.lock(); int m = mode; poseSendingMutex.unlock(); if(m == ON_DEMAND_POSE_SENDING){ doOnDemandPoseSending(); } else if(m == CONTINUOUS_POSE_SENDING){ doContinuousPoseSending(); } else { break; } } finalizeTimer(); } void GRobotController::doOnDemandPoseSending() { while(true){ { unique_lock lock(poseSendingMutex); poseSendingCondition.wait(lock); if(mode != ON_DEMAND_POSE_SENDING){ break; } for(int i=0; i < NJOINTS; ++i){ setJointAngleCommand(i, jointAngles[i], transitionTime); } } poseCommand[109] = calcSum(&poseCommand[4], 105); sendData(&poseCommand[0], 110); } } #ifdef WIN32 void GRobotController::doContinuousPoseSending() { if(isTimerActive){ stopTimer(); } poseSendingMutex.lock(); double ts = motions[currentMotionId].timeStep; poseSendingMutex.unlock(); if(!startTimer(ts)){ poseSendingMutex.lock(); mode = ON_DEMAND_POSE_SENDING; poseSendingMutex.unlock(); return; } while(true){ { unique_lock lock(poseSendingMutex); poseSendingCondition.wait(lock); if(mode != CONTINUOUS_POSE_SENDING){ break; } } } stopTimer(); } bool GRobotController::onMotionFrameSendingRequest() { #ifdef WIN32 LARGE_INTEGER pc1; if(MEASURE_TIMER_INTERVALS){ QueryPerformanceCounter(&pc1); } #else struct timeval tv1; if(MEASURE_TIMER_INTERVALS){ gettimeofday(&tv1, 0); } #endif bool doContinue = true; poseSendingMutex.lock(); MotionInfo& motion = motions[currentMotionId]; if(currentFrame >= motion.numFrames){ doContinue = false; mode = ON_DEMAND_POSE_SENDING; poseSendingMutex.unlock(); poseSendingCondition.notify_all(); } else { double* angles = &motion.angles[currentFrame * NJOINTS]; for(int i=0; i < NJOINTS; ++i){ setJointAngleCommand(i, angles[i], motion.timeStep); } currentFrame++; poseSendingMutex.unlock(); poseCommand[109] = calcSum(&poseCommand[4], 105); sendData(&poseCommand[0], 110); } if(MEASURE_TIMER_INTERVALS){ #ifdef WIN32 cout << "time: " << (double)(pc1.QuadPart - pc0.QuadPart) / pcfreq.QuadPart << endl; #else cout << "time: " << ((tv1.tv_sec - tv0.tv_sec) + (tv1.tv_usec - tv0.tv_usec) / 1000000.0) << endl; #endif } return doContinue; } #else void GRobotController::doContinuousPoseSending() { if(isTimerActive){ stopTimer(); } poseSendingMutex.lock(); double ts = motions[currentMotionId].timeStep; poseSendingMutex.unlock(); if(!startTimer(ts)){ poseSendingMutex.lock(); mode = ON_DEMAND_POSE_SENDING; poseSendingMutex.unlock(); return; } while(true){ sem_wait(&semTimer); if(mode != CONTINUOUS_POSE_SENDING){ break; } struct timeval tv1; if(MEASURE_TIMER_INTERVALS){ gettimeofday(&tv1, 0); } poseSendingMutex.lock(); MotionInfo& motion = motions[currentMotionId]; if(currentFrame >= motion.numFrames){ mode = ON_DEMAND_POSE_SENDING; poseSendingMutex.unlock(); break; } else { double* angles = &motion.angles[currentFrame * NJOINTS]; for(int i=0; i < NJOINTS; ++i){ setJointAngleCommand(i, angles[i], motion.timeStep); } currentFrame++; poseSendingMutex.unlock(); poseCommand[109] = calcSum(&poseCommand[4], 105); sendData(&poseCommand[0], 110); } if(MEASURE_TIMER_INTERVALS){ cout << "time: " << ((tv1.tv_sec - tv0.tv_sec) + (tv1.tv_usec - tv0.tv_usec) / 1000000.0) << endl; } } stopTimer(); while(sem_trywait(&semTimer) == 0); } #endif void GRobotController::requestToSendPose(double transitionTime) { if(makeConnection()){ poseSendingMutex.lock(); if(poseSendingThread == thread()){ poseSendingThread = thread(bind(&GRobotController::poseSendingLoop, this)); } if(mode == ON_DEMAND_POSE_SENDING){ jointAngles = tmpJointAngles; this->transitionTime = transitionTime; poseSendingMutex.unlock(); poseSendingCondition.notify_all(); } else { poseSendingMutex.unlock(); } } } void GRobotController::switchServos(bool on) { poseSendingMutex.lock(); if(makeConnection()){ for(int i=0; i < NJOINTS; ++i){ switchServo(i, on); } } poseSendingMutex.unlock(); } bool GRobotController::setMotion(double* angles, int numFrames, double timeStep, int id) { poseSendingMutex.lock(); if(mode == CONTINUOUS_POSE_SENDING){ poseSendingMutex.unlock(); return false; } if(id >= static_cast(motions.size())){ motions.resize(id + 1); } MotionInfo& motion = motions[id]; if(motion.angles){ delete[] motion.angles; } motion.angles = new double[NJOINTS * numFrames]; std::copy(angles, angles + NJOINTS * numFrames, motion.angles); motion.numFrames = numFrames; motion.timeStep = timeStep; poseSendingMutex.unlock(); return true; } bool GRobotController::startMotion(double time, int id) { bool ready = false; poseSendingMutex.lock(); if(poseSendingThread == thread()){ poseSendingThread = thread(bind(&GRobotController::poseSendingLoop, this)); } if(mode == CONTINUOUS_POSE_SENDING){ poseSendingMutex.unlock(); return false; } currentMotionId = id; MotionInfo& info = motions[id]; currentFrame = static_cast(time / info.timeStep); mode = CONTINUOUS_POSE_SENDING; poseSendingMutex.unlock(); poseSendingCondition.notify_all(); return true; } bool GRobotController::isPlayingMotion() { bool isPlaying = false; poseSendingMutex.lock(); isPlaying = (mode == CONTINUOUS_POSE_SENDING); poseSendingMutex.unlock(); return isPlaying; } void GRobotController::stopMotion() { poseSendingMutex.lock(); mode = ON_DEMAND_POSE_SENDING; poseSendingMutex.unlock(); poseSendingCondition.notify_all(); } choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotController.h000066400000000000000000000055611207742442300233500ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include #include #include #include #include #include #ifndef WIN32 #include #include #endif class GRobotController { public: GRobotController(); GRobotController(const GRobotController& org); virtual ~GRobotController(); int numJoints() const; void setPortDevice(const std::string& device); const std::string& portDevice() const; void switchServos(bool on); void setJointAngle(int jointId, double q) { if(jointId < static_cast(tmpJointAngles.size())){ tmpJointAngles[jointId] = q; } } void requestToSendPose(double transitionTime); bool setMotion(double* angles, int numFrames, double timeStep, int id = 0); bool startMotion(double time = 0.0, int id = 0); bool isPlayingMotion(); void stopMotion(); private: std::string portDevice_; boost::asio::io_service io; boost::asio::serial_port port; std::vector poseCommand; double transitionTime; std::vector tmpJointAngles; std::vector jointAngles; boost::thread poseSendingThread; boost::mutex poseSendingMutex; boost::condition_variable poseSendingCondition; enum { ON_DEMAND_POSE_SENDING, CONTINUOUS_POSE_SENDING, EXIT_POSE_SENDING } mode; struct MotionInfo { MotionInfo() { angles = 0; numFrames = 0; } ~MotionInfo(){ if(angles){ delete[] angles; } } double* angles; double timeStep; int numFrames; }; std::vector motions; int currentMotionId; int currentFrame; double timeStep; bool isTimerAvailable; bool isTimerActive; #ifdef WIN32 HANDLE timerHandle; HANDLE timerQueue; LARGE_INTEGER pc0, pc1, pcfreq; #else sem_t semTimer; timer_t timerID; struct timeval tv0; #endif void init(); bool openSerialPort(); void closeSerialPort(); bool sendData(unsigned char* data, int len); bool receiveData(char* buf, int len); bool checkConnection(); bool makeConnection(); char calcSum(unsigned char* data, int len); void switchServo(int jointId, bool on); void setJointAngleCommand(int jointId, double q, double ttime); void initializeTimer(); bool startTimer(double interval); void stopTimer(); void finalizeTimer(); #if WIN32 static VOID CALLBACK timerCallback(PVOID lpParameter, BOOL TimerOrWaitFired); #else static void timerHandler(int sig, siginfo_t* si, void* uc); #endif void poseSendingLoop(); void doOnDemandPoseSending(); void doContinuousPoseSending(); bool onMotionFrameSendingRequest(); }; choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotControllerItem.cpp000066400000000000000000000116101207742442300245120ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "GRobotControllerItem.h" #include "GRobotBar.h" #include "GRobotController.h" #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; GRobotControllerItem::GRobotControllerItem() : mv(MessageView::mainInstance()) { controller = new GRobotController(); } GRobotControllerItem::GRobotControllerItem(const GRobotControllerItem& org) : Item(org), mv(MessageView::mainInstance()) { controller = new GRobotController(*org.controller); } GRobotControllerItem::~GRobotControllerItem() { delete controller; } ItemPtr GRobotControllerItem::doDuplicate() const { return new GRobotControllerItem(*this); } void GRobotControllerItem::onConnectedToRoot() { checkToggledConnection = ItemTreeView::mainInstance()->sigCheckToggled(this).connect( bind(&GRobotControllerItem::onCheckToggled, this, _1)); } void GRobotControllerItem::onDisconnectedFromRoot() { connections.disconnect(); checkToggledConnection.disconnect(); setSyncMode(false); } void GRobotControllerItem::onCheckToggled(bool on) { if(connections.empty() && on){ GRobotBar* bar = GRobotBar::instance(); connections.add( bar->sigServoSwitchRequest().connect( bind(&GRobotController::switchServos, controller, _1))); connections.add( bar->sigPoseSendRequest().connect( bind(&GRobotControllerItem::requestToSendPose, this, 1.0))); connections.add( bar->sigSyncModeToggled().connect( bind(&GRobotControllerItem::setSyncMode, this, _1))); if(bar->isSyncMode()){ setSyncMode(true); } } else { connections.disconnect(); setSyncMode(false); } } void GRobotControllerItem::onPositionChanged() { bodyItem = findOwnerItem(); } void GRobotControllerItem::setSyncMode(bool on) { kinematicStateChangeConnection.disconnect(); playbackInitilizeConnection.disconnect(); if(on && bodyItem){ kinematicStateChangeConnection = bodyItem->sigKinematicStateChanged().connect( bind(&GRobotControllerItem::requestToSendPose, this, 0.1)); requestToSendPose(1.0); playbackInitilizeConnection = TimeBar::instance()->sigPlaybackInitialized().connect( bind(&GRobotControllerItem::onPlaybackInitialized, this, _1)); } } void GRobotControllerItem::requestToSendPose(double transitionTime) { if(bodyItem){ Body* body = bodyItem->body(); for(int i=0; i < body->numJoints(); ++i){ controller->setJointAngle(i, body->joint(i)->q); } controller->requestToSendPose(transitionTime); } } bool GRobotControllerItem::onPlaybackInitialized(double time) { if(controller->isPlayingMotion()){ controller->stopMotion(); } playbackConnections.disconnect(); if(bodyItem){ BodyMotionItemPtr motionItem = ItemTreeView::mainInstance()->selectedSubItem(bodyItem); if(motionItem){ MultiValueSeqPtr qseq = motionItem->jointPosSeq(); if(qseq->numFrames() > 0 && qseq->numParts() == controller->numJoints()){ if(controller->setMotion(&qseq->at(0, 0), qseq->numFrames(), qseq->getTimeStep())){ TimeBar* timeBar = TimeBar::instance(); playbackConnections.add( timeBar->sigPlaybackStarted().connect( bind(&GRobotControllerItem::onPlaybackStarted, this, _1))); playbackConnections.add( timeBar->sigPlaybackStopped().connect( bind(&GRobotControllerItem::onPlaybackStopped, this))); } } } } return true; } void GRobotControllerItem::onPlaybackStarted(double time) { controller->startMotion(time); } void GRobotControllerItem::onPlaybackStopped() { controller->stopMotion(); playbackConnections.disconnect(); } void GRobotControllerItem::doPutProperties(PutPropertyFunction& putProperty) { putProperty(_("Port"), controller->portDevice(), bind(&GRobotControllerItem::onPortPropertyChanged, this, _1)); } bool GRobotControllerItem::onPortPropertyChanged(const std::string& port) { controller->setPortDevice(port); return true; } bool GRobotControllerItem::store(Archive& archive) { archive.write("port", controller->portDevice()); return true; } bool GRobotControllerItem::restore(const Archive& archive) { controller->setPortDevice(archive.get("port", controller->portDevice())); return true; } choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotControllerItem.h000066400000000000000000000031611207742442300241610ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_GROBOT_CONTROLLER_ITEM_H_INCLUDED #define CNOID_GROBOT_CONTROLLER_ITEM_H_INCLUDED #include #include class GRobotController; namespace cnoid { class MessageView; class BodyItem; class GRobotControllerItem : public Item { public: GRobotControllerItem(); GRobotControllerItem(const GRobotControllerItem& org); ~GRobotControllerItem(); protected: virtual ItemPtr doDuplicate() const; virtual void onConnectedToRoot(); virtual void onDisconnectedFromRoot(); virtual void onPositionChanged(); virtual void doPutProperties(PutPropertyFunction& putProperty); virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); private: GRobotController* controller; BodyItem* bodyItem; ConnectionSet connections; boost::signals::connection checkToggledConnection; boost::signals::connection kinematicStateChangeConnection; boost::signals::connection playbackInitilizeConnection; ConnectionSet playbackConnections; MessageView* mv; void onCheckToggled(bool on); void setSyncMode(bool on); void requestToSendPose(double transitionTime); bool onPlaybackInitialized(double time); void onPlaybackStarted(double time); bool onPortPropertyChanged(const std::string& port); void onPlaybackStopped(); }; typedef boost::intrusive_ptr GRobotControllerItemPtr; } #endif choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotCustomizer.cpp000066400000000000000000000330141207742442300237160ustar00rootroot00000000000000 #ifdef _MSC_VER #ifndef _USE_MATH_DEFINES #define _USE_MATH_DEFINES #endif #endif #include #include #include #include #include #include #include #include #include #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) #define DLL_EXPORT __declspec(dllexport) #define DLL_IMPORT __declspec(dllimport) #else #define DLL_EXPORT #define DLL_IMPORT #endif using namespace std; using namespace boost; using namespace Eigen; using namespace cnoid; namespace { const double l0 = 0.0221; const double l1 = 0.028; const double lo1 = 0.0025; const double l2 = 0.029; const double lo2 = 0.005; const double l3 = 0.048; const double l4 = 0.059; const double lo5 = 0.0005; } inline double degree(double rad) { return (180.0 * rad / M_PI); } inline double radian(double deg) { return (M_PI * deg / 180.0); } typedef Matrix Vector6d; typedef Matrix Matrix6d; static BodyInterface* bodyInterface = 0; static BodyCustomizerInterface bodyCustomizerInterface; enum ModelType { GR001 = 1 }; enum IkType { WAIST_TO_RFOOT = 1, WAIST_TO_LFOOT, NUM_IK_TYPES }; struct JointPathValSet { double* anglePtrs[6]; }; struct GRobotCustomizer { BodyHandle bodyHandle; int modelType; JointPathValSet jointPathValSets[NUM_IK_TYPES]; typedef function IkFunc; IkFunc ikFuncs[NUM_IK_TYPES]; }; class IKSolver { public: const Vector3d& p; const Matrix3d& R; const double sig; Vector6d q; Vector3d rpy; double cy; double sy; double cp; double sp; double cr; double sr; double slo1; Matrix4d Af; Matrix4d TB0; Matrix4d TB1; Matrix4d TB2; Matrix4d TB3; Matrix4d TB4; Matrix4d TB5; Matrix4d TB6; Matrix4d TBF; Vector6d dp; Vector6d dq; IKSolver(const Vector3& p, const Matrix3& R, const double& sig) : p(p), R(R), sig(sig) { } bool calcEndPositionDifference() { // Direct Kinematics Error Calculation double c1 = cos(q[0]); double s1 = sin(q[0]); double c2 = cos(q[1]); double s2 = sin(q[1]); double c3 = cos(q[2]); double s3 = sin(q[2]); double c4 = cos(q[3]); double s4 = sin(q[3]); double c5 = cos(q[4]); double s5 = sin(q[4]); double c6 = cos(q[5]); double s6 = sin(q[5]); Matrix4d A1, A2, A3, A4, A5, A6; A1 << c1, 0.0, -s1, -slo1 * c1, s1, 0.0, c1, -slo1 * s1, 0.0, -1.0, 0.0, -l1, 0.0, 0.0, 0.0, 1.0; A2 << -s2, 0.0, sig * c2, -lo2 * s2, c2, 0.0, sig * s2, lo2 * c2, 0.0, sig, 0.0, -l2, 0.0, 0.0, 0.0, 1.0; A3 << c3, -s3, 0.0, l3 * c3, s3, c3, 0.0, l3 * s3, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; A4 << c4, s4, 0.0, l4 * c4, s4, -c4, 0.0, l4 * s4, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0; A5 << c5, 0.0, -sig * s5, -lo5 * c5, s5, 0.0, sig * c5, -lo5 * s5, 0.0, -sig, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; A6 << -s6, 0.0, -c6, 0.0, c6, 0.0, -s6, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; TB1.noalias() = TB0 * A1; TB2.noalias() = TB1 * A2; TB3.noalias() = TB2 * A3; TB4.noalias() = TB3 * A4; TB5.noalias() = TB4 * A5; TB6.noalias() = TB5 * A6; TBF.noalias() = TB6 * Af; double yaw = atan2(TBF(1,0), TBF(0,0)); double pitch = asin(-TBF(2,0)); double roll = atan2(TBF(2,1), TBF(2,2)); double dYaw = rpy[2] - yaw; double dPitch = rpy[1] - pitch; double dRoll = rpy[0] - roll; Vector3 pi = TBF.block<3,1>(0,3); dp << (p - pi), (-sy * dPitch + cy * cp * dRoll), (cy * dPitch + sy * cp * dRoll), (dYaw - sp * dRoll); double errsqr = dp.head(3).squaredNorm() + dp.tail(3).squaredNorm(); return (errsqr < 1.0e-6 * 1.0e-6); } bool calcLegAngles(double** out_q) { rpy = R.eulerAngles(2, 1, 0).reverse(); cy = cos(rpy[2]); sy = sin(rpy[2]); cp = cos(rpy[1]); sp = sin(rpy[1]); cr = cos(rpy[0]); sr = sin(rpy[0]); Vector3d O1(0.0, sig * l0, -l1); const Vector3d& F = p; Vector3d V1 = F - O1; Vector3d XF(cy * cp, sy * cp, -sp); Vector3d V1xXF = V1.cross(XF); Vector3d Z2 = -sig * V1xXF / V1xXF.norm(); q[0] = atan2(-sig * Z2[0], sig * Z2[1]); q[1] = asin(-sig * Z2[2]); double c1 = cos(q[0]); double s1 = sin(q[0]); double c2 = cos(q[1]); double s2 = sin(q[1]); double s1s2 = s1 * s2; double c1s2 = c1 * s2; slo1 = sig * lo1; TB2 << s1s2, -sig * c1, -sig * s1 * c2, slo1 * s1 + l2 * c1 + lo2 * s1s2, -c1s2, -sig * s1, sig * c1 * c2, sig * l0 - slo1 * c1 + l2 * s1 - lo2 * c1s2, -c2, 0.0, -sig * s2, -l1 - lo2 * c2, 0.0, 0.0, 0.0, 1.0; Vector3d V2 = (TB2.inverse() * Vector4d(F[0], F[1], F[2], 1.0)).head(3); double D = (V2.squaredNorm() - l3 * l3 - l4 * l4) / (2.0 * l3 * l4); if(fabs(D) > 1.0){ return false; } q[3] = atan2(-sig * sqrt(1.0 - D * D), D); double c4 = cos(q[3]); double s4 = sin(q[3]); double beta = atan2(-V2[1], sqrt(V2[0] * V2[0] + V2[2] * V2[2])); double alpha = atan2(l4 * s4, l3 + l4 * c4); q[2] = -(beta - alpha); q[3] = -q[3]; double c3 = cos(q[2]); double s3 = sin(q[2]); double q2q3 = q[2] + q[3]; double c34 = cos(q2q3); double s34 = sin(q2q3); Matrix4d T24; T24 << c34, s34, 0, l3 * c3 + l4 * c34, s34, -c34, 0, l3 * s3 + l4 * s34, 0.0, 0.0, -1, 0.0, 0.0, 0.0, 0, 1.0; TB4.noalias() = TB2 * T24; double spsr = sp * sr; double spcr = sp * cr; TBF << cy * cp, -sy * cr + cy * spsr, sy * sr + cy * spcr, p.x(), sy * cp, cy * cr + sy * spsr, -cy * sr + sy * spcr, p.y(), -sp, cp * sr, cp * cr, p.z(), 0, 0, 0, 1.0; Matrix4d T4F; T4F.noalias() = TB4.inverse() * TBF; q[4] = atan2(-sig * T4F(0,0), sig * T4F(1,0)); q[5] = atan2( sig * T4F(2,2), -sig * T4F(2,1)); // Numerical refining TB0 << 0.0, -1.0, 0.0, 0.0, 1.0, 0.0, 0.0, sig * l0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; Af << 0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; bool solved = false; int i; for(i=0; i < 30; ++i){ if(calcEndPositionDifference()){ solved = true; break; } // Jacobian Calculation Vector3d Z0 = TB0.block<3,1>(0,2); Vector3d Z1 = TB1.block<3,1>(0,2); Vector3d Z2 = TB2.block<3,1>(0,2); Vector3d Z3 = TB3.block<3,1>(0,2); Vector3d Z4 = TB4.block<3,1>(0,2); Vector3d Z5 = TB5.block<3,1>(0,2); Vector3d O0 = TB0.block<3,1>(0,3); Vector3d O1 = TB1.block<3,1>(0,3); Vector3d O2 = TB2.block<3,1>(0,3); Vector3d O3 = TB3.block<3,1>(0,3); Vector3d O4 = TB4.block<3,1>(0,3); Vector3d O5 = TB5.block<3,1>(0,3); Vector3d O6 = TB6.block<3,1>(0,3); Matrix6d J; J << Z0.cross(O6 - O0), Z1.cross(O6 - O1), Z2.cross(O6 - O2), Z3.cross(O6 - O3), Z4.cross(O6 - O4), Z5.cross(O6 - O5), Z0, Z1, Z2, Z3, Z4, Z5 ; // Levenberg-Marquardt Method const double lambda = 0.001; Matrix6d C; C.noalias() = J.transpose() * (J * J.transpose() + Matrix6d::Identity() * lambda * lambda).inverse(); dq.noalias() = C * dp; q += dq; if(dq.norm() <= 1.0e-5){ break; } } if(!solved){ solved = calcEndPositionDifference(); } if(solved){ *out_q[0] = q[0]; *out_q[1] = q[1]; *out_q[2] = q[2]; *out_q[3] = q[3]; *out_q[4] = q[4]; *out_q[5] = q[5]; } return solved; } }; static const char* gr001RightLegLinks[] = { "R_HIP_Y", "R_HIP_R", "R_HIP_P", "R_KNEE_P", "R_ANKLE_P", "R_ANKLE_R" }; static const char* gr001LeftLegLinks[] = { "L_HIP_Y", "L_HIP_R", "L_HIP_P", "L_KNEE_P", "L_ANKLE_P", "L_ANKLE_R" }; static const char** getTargetModelNames() { static const char* names[] = { "GR001", 0 }; return names; } static int initializeIkPath(GRobotCustomizer* customizer, int ikType, const char* linkNames[]) { BodyHandle body = customizer->bodyHandle; JointPathValSet& pathValSet = customizer->jointPathValSets[ikType]; for(int i=0; i < 6; ++i){ int linkIndex = bodyInterface->getLinkIndexFromName(body, linkNames[i]); if(linkIndex < 0){ ikType = 0; break; } pathValSet.anglePtrs[i] = bodyInterface->getJointValuePtr(body, linkIndex); } return ikType; } static bool calcLegAngles(const Vector3& p, const Matrix3& R, double sig, double** out_q) { IKSolver solver(p, R, sig); return solver.calcLegAngles(out_q); } static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName) { GRobotCustomizer* customizer = 0; int modelType = 0; string name(modelName); if(name == "GR001"){ modelType = GR001; } if(modelType){ customizer = new GRobotCustomizer; customizer->modelType = modelType; customizer->bodyHandle = bodyHandle; switch(modelType){ case GR001: if(initializeIkPath(customizer, WAIST_TO_RFOOT, gr001RightLegLinks)){ customizer->ikFuncs[WAIST_TO_RFOOT] = bind(calcLegAngles, _1, _2, -1.0, customizer->jointPathValSets[WAIST_TO_RFOOT].anglePtrs); } if(initializeIkPath(customizer, WAIST_TO_LFOOT, gr001LeftLegLinks)){ customizer->ikFuncs[WAIST_TO_LFOOT] = bind(calcLegAngles, _1, _2, 1.0, customizer->jointPathValSets[WAIST_TO_LFOOT].anglePtrs); } break; default: break; } } return static_cast(customizer); } static void destroy(BodyCustomizerHandle customizerHandle) { GRobotCustomizer* customizer = static_cast(customizerHandle); if(customizer){ delete customizer; } } static inline bool match(const char* s1, const char* s2) { return (strcmp(s1, s2) == 0); } static int initializeIk(BodyCustomizerHandle customizerHandle, int baseLinkIndex, int targetLinkIndex) { GRobotCustomizer* customizer = static_cast(customizerHandle); int ikType = 0; const char* baseLinkName = bodyInterface->getLinkName(customizer->bodyHandle, baseLinkIndex); const char* targetLinkName = bodyInterface->getLinkName(customizer->bodyHandle, targetLinkIndex); switch (customizer->modelType){ case GR001: if(match(baseLinkName, "WAIST")){ if(match(targetLinkName, "R_ANKLE_R")){ ikType = WAIST_TO_RFOOT; } // Currently disabled because the IKFast generated code does not work well else if(match(targetLinkName, "L_ANKLE_R")){ ikType = WAIST_TO_LFOOT; } //*/ } break; default: break; } if(!customizer->ikFuncs[ikType]){ ikType = 0; } return ikType; } static bool calcAnalyticIk(BodyCustomizerHandle customizerHandle, int ikType, const Vector3d& p, const Matrix3& R) { if(ikType < 0 && ikType >= NUM_IK_TYPES){ return false; } GRobotCustomizer* customizer = static_cast(customizerHandle); if(customizer->ikFuncs[ikType]){ return customizer->ikFuncs[ikType](p, R); } return false; } extern "C" DLL_EXPORT BodyCustomizerInterface* getHrpBodyCustomizerInterface(BodyInterface* bodyInterface_) { bodyInterface = bodyInterface_; bodyCustomizerInterface.version = BODY_CUSTOMIZER_INTERFACE_VERSION; bodyCustomizerInterface.getTargetModelNames = getTargetModelNames; bodyCustomizerInterface.create = create; bodyCustomizerInterface.destroy = destroy; bodyCustomizerInterface.initializeAnalyticIk = initializeIk; bodyCustomizerInterface.calcAnalyticIk = calcAnalyticIk; bodyCustomizerInterface.setVirtualJointForces = 0; return &bodyCustomizerInterface; } choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotPlugin.cpp000066400000000000000000000023661207742442300230160ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "GRobotBar.h" #include "GRobotControllerItem.h" #include #include #include #include #include #include "gettext.h" namespace cnoid { class GRobotPlugin : public Plugin { public: GRobotPlugin() : Plugin("GRobot") { require("Body"); } virtual bool initialize() { addToolBar(GRobotBar::instance()); itemManager().registerClass(N_("GRobotControllerItem")); itemManager().addCreationPanel(); return true; } virtual const char* description() { static std::string text = str(boost::format(_("GRobot Plugin Version %1%\n")) % CNOID_FULL_VERSION_STRING) + "\n" + _("This plugin has been developed by Shin'ichiro Nakaoka and Choreonoid Development Team, AIST, " "and is distributed as a part of the Choreonoid package.\n" "\n") + LGPLtext(); return text.c_str(); } }; } CNOID_IMPLEMENT_PLUGIN_ENTRY(cnoid::GRobotPlugin) choreonoid-1.1.0+dfsg/src/GRobotPlugin/GRobotPlugin.qrc000066400000000000000000000003541207742442300230140ustar00rootroot00000000000000 icons/grobo-logo.png icons/servo-on.png icons/sendpose.png icons/syncpose.png choreonoid-1.1.0+dfsg/src/GRobotPlugin/icons/000077500000000000000000000000001207742442300210435ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/GRobotPlugin/icons/grobo-logo.png000066400000000000000000000012421207742442300236160ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<IDATH‰Õ–ÏkQÇ?o7´AE"[y  Ä‹xi6íEAñ¯hšh˜(ùTkM¸$"Ÿ›‘ ©T¸¥Æ˜h>áe6•Ë4c㉯´‚çƒà<Ƽ´²RVõ­µ§Z½ÉD$Û¬K§/¯·ƒ|YZº|kp°]äƒà¢§Àʸªê¾°Ö ø‘n…nW©\.ôXk¯¢µád’³ñx=±!¶+kmÐ Œ„¹¤ïÇOtu]‡¹–ö{G|¿{¦¯¯·™ÀKkÆ6AÆô¨1Ï£ðZA\ƒÀP"ÁP"qÎÁïûjŒrí4º=ùd+ž‹ÐÌA»1U*1^ŸI“1àç¼s+Îj‹¬ÊT©Ä»ÊæT¹#"wªb­}\oK +À ˆL@ýMß;$P‘áÂü÷îз¬×2àIEND®B`‚choreonoid-1.1.0+dfsg/src/GRobotPlugin/icons/icons.svg000066400000000000000000000624651207742442300227140ustar00rootroot00000000000000 Icons of Choreonoid GRobotPlugin image/svg+xml Icons of Choreonoid GRobotPlugin Shin'ichiro Nakaoka choreonoid-1.1.0+dfsg/src/GRobotPlugin/icons/sendpose.png000066400000000000000000000014721207742442300233750ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<·IDATH‰í•KHTQÇÿçÜ;L35Rt(CÝè¨Tb­#H‚@!ßià"kéÆ»ÚE›°ˆtÔì7†½ˆZ¨Y6˜f…áTvsÆ™;÷ܯE86:ãÕlÙ·<ßã÷ýÏwŒˆ°c Ì^ÑÑÂ%–lŠò5´5z7•·€½ìžY2/;ˆPf’¹.trr¿Vôêný7£\nWv-V²ø2ŽÒ¦ò|Ör¶@ŠÚaÊÑ-ò {MWú¶äVwÛÌ&9½©üˆœf‹Ì»½¸rkPû¾àYÒ4V4ÞS3´e@Nu{6ã| &ʲç\éQ91!:è#^_×{‡ÅÇ9EÓ²7u}›Ø«…c}{¢­ %yò®Ö°MhšŽžþQ}|ÚÎu×]5Ø+;*8cŽÔ”8^qò·˜åHê«!Bÿ³I }1vq¼»¶…Á¢!€ìÊöf0~)3- § ³˜Ä9Bð¸Aç<žºÍÑþú‘¶Æ@ÀZ[¹}êÀe]ȳïGA^Ɔ]G²Éé9Ü>®3ÂS/¤â©®ªEv¸±Íä÷X$%1YÉÁ‹Iƾ䄈 À×ùE¸—–ƒk³s &Þc˜Ð˜t\ÖÜÖ¨f] f]JH‘9°%Ň,/«èx©“Nëî2eÇä×·kßåÖvÆpI˜þ ð«"V‚<ãñàWõ°¯_éĉpÚd¡G!p®‹·7~ÊàtÔxÖ&¬o‡¦šÐá„Vfí×VÖÉ3zóÌB¸&6>ƒTAðÂ+PÆï˜!@´ª`M=5xK€€¦Ãa«[´-­›ÁŠýB'h"ü^ ãúÆÿÁví?ÀÐ ‡ìv»¡ªjXŸâï»gR—~Ø>?V%_Q”Ha`œ¹$g$ÿ/2?Ôö!‚9IEND®B`‚choreonoid-1.1.0+dfsg/src/GRobotPlugin/icons/servo-on.png000066400000000000000000000031331207742442300233210ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<ØIDATH‰“[L÷Æ3;;˲ ¬a%Xb»æ²&eÉ´•j6®‹eƒ/…ʸRUò·ŠZ»/iâËC*Õ²±EE*²Õö!*ÎCµf±±!µÕzI— âRÕ–q‘q »;{a˜ý÷3 qšúHŸfæ̾ï|çÌ‘„¿%êâÝK†‚x¥Ž7€.þ=q•·ß„-¥ðëw–EÞ<õV+ÿyõõÄÉ“'3€ª(Ê?²Ùlèƒ ž†$VVr¼ì“øç]xqÌÍÂò¼ö3x»û3F†ËiÞ½vúOƒðí½Ðð5°å1óþ °iÓ&‰ƒÁÁÁÌÄÄÄ[ÊSó·¤[l­¤­­Í›Ëå>‘e9/£i™L†X,FYYíííÔÖÖâp8Ð4íK«ÇãLOO’$}ª|¾Å---ͪªÞ,..¶455Q^^N~~>ùùùH’ÄÝ»wéééaß¾}=zô™¾¯chh(¥ëúÍ`0ø±)°ÿþB›ÍvsçοßÅbazzšd2IAA•••êëë¹xñ"‡ŸÏ·¡rMÓˆD"+sss÷³ÙìqSÀjµžq¹\zUU•%•J±°°@*•"N“N§¹sç ´µµÑÕÕÅ… p»Ý¨ªjV>>>ž{øðá¢a7ndMóçÏËÀO¼^¯M’$VWWQUÃ0B˜¦¨¨¿ßÏç#‰àóùÐ4ÉÉIFGGSº®7…B¡ysÛ¢Ñè‹ÅbØl6yyy&l6›‰¡¡!4MÃãñðèÑ#R©³³³Ü¾};-„h …Bÿúü\I’¾ît: »Ý€®ë„ÃaÊÊÊ$‰ÒÒR„¤Ói¦§§Ù¼y3333Äb1S†aœƒa¾òÓ•_XYY«««äåå¡( ýýý8p€@ @,Ãf³™®Òé4±X UU ‡Ãš®ëWƒÁ`ÏÉM‹%šÉdìÉdÃ0èëë#àv»éííeïÞ½æ<$IB’$¦¦¦Bñx¬¯¯ÿÃÙ³gsÏC UWW;«ªª>ž™™1VVV:‡‡‡<ïÇÏÿBpÿL7ÑIEND®B`‚choreonoid-1.1.0+dfsg/src/GRobotPlugin/icons/syncpose.png000066400000000000000000000017101207742442300234130ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<EIDATH‰¥•OlGÆ¿™Ùuü'ª1JIª’HÀÖ1–rHUQµäF¥©Úàr@”nÜÊC¥ª9áPUU!q©%´¥—¤±…˜Z†"ÙˆMj{Ó}3œjÙhgÔwšÕ÷Þû½73o‡)¥°#cŒ¥-ëã| išçg2™; Û à{Ë 4„˜ƒR§„iJIô§Ëù‰Ôýû•íbùv³Éd´)ÄOŒ±Éc©›8wNB¡CÊ…o-kÿÿêàZ"ñ&·Ì@`ÿ{SSÆž}ûµ ÜJ§ÝªmטëžøteåÞk®ŒŒÄc7ƒápÏû““Ft÷îÝi6qûúuª”J.¤<5ËÝØ1`βÞeœßˆîÚŸ˜0Âáp[k-‰¿ÎÏËÇÅ"œÎf¿Þ¶¬çs}½½üøØ7MS×=@ø}qkù<˜R_~¶²rmI;Wâñó¸4Ôß·-‹qþÊh«þUË øc}®9]]©™Læßà"c|ز.+à‹ÃýýH ùV­*üöð¡„R¿ˆfóäT>ÿûæèQ3è8ß)àý‘Æb­€çìîöíàY£êÖVë»T¯#oÛàÀ*#7‚®; à$”ëu”ëõŽã== =háÇRIJy’À!Æq¦”ÂÕD"Bœwœ&9NÔ¢øN(„aÃð¬¾&%~¨Õ¥>R¦y»ÐhÐçkkUíÌ&“Ñ.ÇÙè(H/˜ >˜ÎfozùžÙÌd2›éxüL•±ÑªÎ —²ôVoï¼N×@r~Ù¾>Bݹãêtí]M$öH¥ž@)ßIcŒSËË/]Ûqf®kŽŽbp`ÀóŸV*øùî]Á”zC—Çw‹àÁÒg³í%·–Ñváz@äÀ¿««_=k6¾[«ZÙï=˜M&£¢˜Ö@€hó“åeíEðƒë>D}~.„M®{èt.WöÒµ[ ŠI¢¾ø‘#èéófl<ŽÅ……€½^ðŸ½xô›å²ç-ª·ýät¦l ñ4ìºKÙöa¿(¸¦YÔé/Btb1`‹ïPIEND®B`‚choreonoid-1.1.0+dfsg/src/GRobotPlugin/po/000077500000000000000000000000001207742442300203465ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/GRobotPlugin/po/ja.po000066400000000000000000000032511207742442300213010ustar00rootroot00000000000000# Japanese translations for PACKAGE package. # Copyright (C) 2011 THE PACKAGE'S COPYRIGHT HOLDER # This file is distributed under the same license as the PACKAGE package. # nakaoka , 2011. # msgid "" msgstr "" "Project-Id-Version: PACKAGE VERSION\n" "Report-Msgid-Bugs-To: \n" "POT-Creation-Date: 2011-12-21 16:06+0000\n" "PO-Revision-Date: 2011-11-15 22:02+0000\n" "Last-Translator: nakaoka \n" "Language-Team: Japanese\n" "Language: ja\n" "MIME-Version: 1.0\n" "Content-Type: text/plain; charset=UTF-8\n" "Content-Transfer-Encoding: 8bit\n" "Plural-Forms: nplurals=1; plural=0;\n" #: GRobotBar.cpp:20 msgid "GRobotBar" msgstr "GRobotãƒãƒ¼" #: GRobotBar.cpp:23 msgid "G-Robot toolbar which provides buttons for handling actual G-Robots" msgstr "G-Robot実機を扱ã†ãƒœã‚¿ãƒ³ã‚’ã¾ã¨ã‚ãŸG-Robotツールãƒãƒ¼" #: GRobotBar.cpp:28 msgid "Turn on / off servo gains" msgstr "サーボON/OFF" #: GRobotBar.cpp:31 msgid "Send the current pose of virtual robots to actual robots" msgstr "ロボットã®å§¿å‹¢ã‚’実機ã«é€ä¿¡" #: GRobotBar.cpp:34 msgid "Synchronize the pose of actual robots pose with virtual robots" msgstr "ロボットã®å§¿å‹¢ã‚’実機ã«åŒæœŸ" #: GRobotControllerItem.cpp:176 msgid "Port" msgstr "ãƒãƒ¼ãƒˆ" #: GRobotPlugin.cpp:29 msgid "GRobotControllerItem" msgstr "GRobotコントローラアイテム" #: GRobotPlugin.cpp:37 msgid "GRobot Plugin Version %1%\n" msgstr "GRobot プラグイン ãƒãƒ¼ã‚¸ãƒ§ãƒ³ %1%\n" #: GRobotPlugin.cpp:39 msgid "" "This plugin has been developed by Shin'ichiro Nakaoka and Choreonoid " "Development Team, AIST, and is distributed as a part of the Choreonoid " "package.\n" "\n" msgstr "" choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/000077500000000000000000000000001207742442300201135ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/BodyMotionGenerationBar.cpp000066400000000000000000000572711207742442300253570ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "BodyMotionGenerationBar.h" #include "PoseSeqItem.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; } namespace cnoid { class BodyMotionGenerationSetupDialog : public QDialog { public: QVBoxLayout* vbox; DoubleSpinBox timeScaleRatioSpin; DoubleSpinBox preInitialDurationSpin; DoubleSpinBox postFinalDurationSpin; CheckBox onlyTimeBarRangeCheck; CheckBox newBodyItemCheck; CheckBox stealthyStepCheck; DoubleSpinBox stealthyHeightRatioThreshSpin; DoubleSpinBox flatLiftingHeightSpin; DoubleSpinBox flatLandingHeightSpin; DoubleSpinBox impactReductionHeightSpin; DoubleSpinBox impactReductionTimeSpin; CheckBox autoZmpCheck; DoubleSpinBox minZmpTransitionTimeSpin; DoubleSpinBox zmpCenteringTimeThreshSpin; DoubleSpinBox zmpTimeMarginBeforeLiftingSpin; CheckBox se3Check; CheckBox lipSyncMixCheck; void addSeparator(QVBoxLayout* vbox){ vbox->addSpacing(4); vbox->addWidget(new HSeparator()); vbox->addSpacing(2); } void addSeparator(QVBoxLayout* vbox, QWidget* widget) { vbox->addSpacing(4); vbox->addLayout(new HSeparatorBox(widget)); vbox->addSpacing(2); } QHBoxLayout* newRow(QVBoxLayout* vbox) { QHBoxLayout* hbox = new QHBoxLayout(); hbox->setSpacing(2); hbox->setContentsMargins(2, 2, 2, 2); vbox->addLayout(hbox); return hbox; } BodyMotionGenerationSetupDialog() : QDialog(MainWindow::instance()){ setWindowTitle(_("Body Motion Generation Setup")); vbox = new QVBoxLayout(); QHBoxLayout* hbox = newRow(vbox); hbox->addWidget(new QLabel(_("Time scale"))); timeScaleRatioSpin.setDecimals(2); timeScaleRatioSpin.setRange(0.01, 9.99); timeScaleRatioSpin.setSingleStep(0.01); timeScaleRatioSpin.setValue(1.0); hbox->addWidget(&timeScaleRatioSpin); hbox->addSpacing(8); hbox->addWidget(new QLabel(_("Pre-initial"))); preInitialDurationSpin.setDecimals(1); preInitialDurationSpin.setRange(0.0, 9.9); preInitialDurationSpin.setSingleStep(0.1); preInitialDurationSpin.setValue(1.0); hbox->addWidget(&preInitialDurationSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox->addSpacing(8); hbox->addWidget(new QLabel(_("Post-final"))); postFinalDurationSpin.setDecimals(1); postFinalDurationSpin.setRange(0.0, 9.9); postFinalDurationSpin.setSingleStep(0.1); postFinalDurationSpin.setValue(1.0); hbox->addWidget(&postFinalDurationSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox->addStretch(); hbox = newRow(vbox); onlyTimeBarRangeCheck.setText(_("Time bar's range only")); onlyTimeBarRangeCheck.setChecked(false); hbox->addWidget(&onlyTimeBarRangeCheck); se3Check.setText(_("Put all link positions")); se3Check.setChecked(false); hbox->addWidget(&se3Check); hbox->addStretch(); hbox = newRow(vbox); newBodyItemCheck.setText(_("Make a new body item")); newBodyItemCheck.setChecked(true); hbox->addWidget(&newBodyItemCheck); hbox->addStretch(); addSeparator(vbox, &stealthyStepCheck); stealthyStepCheck.setText(_("Stealthy Step Mode")); stealthyStepCheck.setToolTip(_("This mode makes foot lifting / landing smoother to increase the stability")); stealthyStepCheck.setChecked(true); hbox = newRow(vbox); hbox->addWidget(new QLabel(_("Height ratio thresh"))); stealthyHeightRatioThreshSpin.setAlignment(Qt::AlignCenter); stealthyHeightRatioThreshSpin.setDecimals(2); stealthyHeightRatioThreshSpin.setRange(1.00, 9.99); stealthyHeightRatioThreshSpin.setSingleStep(0.01); stealthyHeightRatioThreshSpin.setValue(2.0); hbox->addWidget(&stealthyHeightRatioThreshSpin); hbox->addStretch(); hbox = newRow(vbox); hbox->addWidget(new QLabel(_("Flat Lifting Height"))); flatLiftingHeightSpin.setAlignment(Qt::AlignCenter); flatLiftingHeightSpin.setDecimals(3); flatLiftingHeightSpin.setRange(0.0, 0.0999); flatLiftingHeightSpin.setSingleStep(0.001); flatLiftingHeightSpin.setValue(0.005); hbox->addWidget(&flatLiftingHeightSpin); hbox->addWidget(new QLabel(_("[m]"))); hbox->addSpacing(8); hbox->addWidget(new QLabel(_("Flat Landing Height"))); flatLandingHeightSpin.setAlignment(Qt::AlignCenter); flatLandingHeightSpin.setDecimals(3); flatLandingHeightSpin.setRange(0.0, 0.0999); flatLandingHeightSpin.setSingleStep(0.001); flatLandingHeightSpin.setValue(0.005); hbox->addWidget(&flatLandingHeightSpin); hbox->addWidget(new QLabel(_("[m]"))); hbox->addStretch(); hbox = newRow(vbox); hbox->addWidget(new QLabel(_("Impact reduction height"))); impactReductionHeightSpin.setAlignment(Qt::AlignCenter); impactReductionHeightSpin.setDecimals(3); impactReductionHeightSpin.setRange(0.0, 0.099); impactReductionHeightSpin.setSingleStep(0.001); impactReductionHeightSpin.setValue(0.005); hbox->addWidget(&impactReductionHeightSpin); hbox->addWidget(new QLabel(_("[m]"))); hbox->addSpacing(8); hbox->addWidget(new QLabel(_("Impact reduction time"))); impactReductionTimeSpin.setAlignment(Qt::AlignCenter); impactReductionTimeSpin.setDecimals(3); impactReductionTimeSpin.setRange(0.001, 0.999); impactReductionTimeSpin.setSingleStep(0.001); impactReductionTimeSpin.setValue(0.04); hbox->addWidget(&impactReductionTimeSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox->addStretch(); addSeparator(vbox, &autoZmpCheck); autoZmpCheck.setText(_("Auto ZMP Mode")); autoZmpCheck.setToolTip(_("Automatically insert ZMP and foot key poses for stable motion")); autoZmpCheck.setChecked(true); hbox = newRow(vbox); hbox->addWidget(new QLabel(_("Min. transtion time"))); minZmpTransitionTimeSpin.setDecimals(2); minZmpTransitionTimeSpin.setRange(0.01, 0.99); minZmpTransitionTimeSpin.setSingleStep(0.01); minZmpTransitionTimeSpin.setValue(0.1); hbox->addWidget(&minZmpTransitionTimeSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox->addSpacing(8); hbox->addWidget(new QLabel(_("Centering time thresh"))); zmpCenteringTimeThreshSpin.setDecimals(3); zmpCenteringTimeThreshSpin.setRange(0.001, 0.999); zmpCenteringTimeThreshSpin.setSingleStep(0.001); zmpCenteringTimeThreshSpin.setValue(0.03); hbox->addWidget(&zmpCenteringTimeThreshSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox->addStretch(); hbox = newRow(vbox); hbox->addWidget(new QLabel(_("Time margin before lifting"))); zmpTimeMarginBeforeLiftingSpin.setDecimals(3); zmpTimeMarginBeforeLiftingSpin.setRange(0.0, 0.999); zmpTimeMarginBeforeLiftingSpin.setSingleStep(0.001); zmpTimeMarginBeforeLiftingSpin.setValue(0.0); hbox->addWidget(&zmpTimeMarginBeforeLiftingSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox->addStretch(); addSeparator(vbox); hbox = newRow(vbox); lipSyncMixCheck.setText(_("Mix lip-sync motion")); lipSyncMixCheck.setChecked(false); hbox->addWidget(&lipSyncMixCheck); hbox->addStretch(); QVBoxLayout* topVBox = new QVBoxLayout(); topVBox->addLayout(vbox); addSeparator(topVBox); QPushButton* okButton = new QPushButton(_("&Ok")); okButton->setDefault(true); QDialogButtonBox* buttonBox = new QDialogButtonBox(this); buttonBox->addButton(okButton, QDialogButtonBox::AcceptRole); connect(buttonBox,SIGNAL(accepted()), this, SLOT(accept())); topVBox->addWidget(buttonBox); setLayout(topVBox); } void storeState(Archive& archive){ archive.write("timeScaleRatio", timeScaleRatioSpin.value()); archive.write("preInitialDuration", preInitialDurationSpin.value()); archive.write("postFinalDuration", postFinalDurationSpin.value()); archive.write("onlyTimeBarRange", onlyTimeBarRangeCheck.isChecked()); archive.write("makeNewBodyItem", newBodyItemCheck.isChecked()); archive.write("stealthyStepMode", stealthyStepCheck.isChecked()); archive.write("stealthyHeightRatioThresh", stealthyHeightRatioThreshSpin.value()); archive.write("flatLiftingHeight", flatLiftingHeightSpin.value()); archive.write("flatLandingHeight", flatLandingHeightSpin.value()); archive.write("impactReductionHeight", impactReductionHeightSpin.value()); archive.write("impactReductionTime", impactReductionTimeSpin.value()); archive.write("autoZmp", autoZmpCheck.isChecked()); archive.write("minZmpTransitionTime", minZmpTransitionTimeSpin.value()); archive.write("zmpCenteringTimeThresh", zmpCenteringTimeThreshSpin.value()); archive.write("zmpTimeMarginBeforeLiftingSpin", zmpTimeMarginBeforeLiftingSpin.value()); archive.write("allLinkPositions", se3Check.isChecked()); archive.write("lipSyncMix", lipSyncMixCheck.isChecked()); } void restoreState(const Archive& archive){ timeScaleRatioSpin.setValue(archive.get("timeScaleRatio", timeScaleRatioSpin.value())); preInitialDurationSpin.setValue(archive.get("preInitialDuration", preInitialDurationSpin.value())); postFinalDurationSpin.setValue(archive.get("postFinalDuration", postFinalDurationSpin.value())); onlyTimeBarRangeCheck.setChecked(archive.get("onlyTimeBarRange", onlyTimeBarRangeCheck.isChecked())); newBodyItemCheck.setChecked(archive.get("makeNewBodyItem", newBodyItemCheck.isChecked())); stealthyStepCheck.setChecked(archive.get("stealthyStepMode", stealthyStepCheck.isChecked())); stealthyHeightRatioThreshSpin.setValue(archive.get("stealthyHeightRatioThresh", stealthyHeightRatioThreshSpin.value())); flatLiftingHeightSpin.setValue(archive.get("flatLiftingHeight", flatLiftingHeightSpin.value())); flatLandingHeightSpin.setValue(archive.get("flatLandingHeight", flatLandingHeightSpin.value())); impactReductionHeightSpin.setValue(archive.get("impactReductionHeight", impactReductionHeightSpin.value())); impactReductionTimeSpin.setValue(archive.get("impactReductionTime", impactReductionTimeSpin.value())); autoZmpCheck.setChecked(archive.get("autoZmp", autoZmpCheck.isChecked())); minZmpTransitionTimeSpin.setValue(archive.get("minZmpTransitionTime", minZmpTransitionTimeSpin.value())); zmpCenteringTimeThreshSpin.setValue(archive.get("zmpCenteringTimeThresh", zmpCenteringTimeThreshSpin.value())); zmpTimeMarginBeforeLiftingSpin.setValue(archive.get("zmpTimeMarginBeforeLiftingSpin", zmpTimeMarginBeforeLiftingSpin.value())); se3Check.setChecked(archive.get("allLinkPositions", se3Check.isChecked())); lipSyncMixCheck.setChecked(archive.get("lipSyncMix", lipSyncMixCheck.isChecked())); } }; } void BodyMotionGenerationBar::initializeInstance(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ BodyMotionGenerationBar* bar = instance(); ext->addToolBar(bar); MenuManager& mm = ext->menuManager(); mm.setPath("/Options").setPath(N_("Pose Seq Processing")); bar->autoInterpolationUpdateCheck = mm.addCheckItem(_("Automatic Interpolation Update")); bar->autoInterpolationUpdateCheck->setChecked(true); mm.addSeparator(); initialized = true; } } BodyMotionGenerationBar* BodyMotionGenerationBar::instance() { static BodyMotionGenerationBar* bar = new BodyMotionGenerationBar(); return bar; } BodyMotionGenerationBar::BodyMotionGenerationBar() : ToolBar("BodyMotionGenerationBar") { bodyMotionPoseProvider = new BodyMotionPoseProvider(); poseProviderToBodyMotionConverter = new PoseProviderToBodyMotionConverter(); timeBar = TimeBar::instance(); setup = new BodyMotionGenerationSetupDialog(); addButton(QIcon(":/PoseSeq/icons/trajectory-generation.png"), _("Generate body motions")) ->sigClicked().connect(bind(&BodyMotionGenerationBar::onGenerationButtonClicked, this)); interpolationParameterWidgetsConnection.add( setup->timeScaleRatioSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->preInitialDurationSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->postFinalDurationSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->onlyTimeBarRangeCheck.sigToggled().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); autoGenerationToggle = addToggleButton(QIcon(":/PoseSeq/icons/auto-update.png"), _("Automatic Balance Adjustment Mode")); autoGenerationToggle->setChecked(false); balancerToggle = addToggleButton(QIcon(":/PoseSeq/icons/balancer.png"), _("Enable the balancer")); balancerToggle->setEnabled(false); balancerToggle->setChecked(false); addButton(QIcon(":/Base/icons/setup.png"))->sigClicked().connect(bind(&BodyMotionGenerationSetupDialog::show, setup)); interpolationParameterWidgetsConnection.add( setup->stealthyStepCheck.sigToggled().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->stealthyHeightRatioThreshSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->flatLiftingHeightSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->flatLandingHeightSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->impactReductionHeightSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->impactReductionTimeSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->autoZmpCheck.sigToggled().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->minZmpTransitionTimeSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->zmpCenteringTimeThreshSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->zmpTimeMarginBeforeLiftingSpin.sigValueChanged().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); interpolationParameterWidgetsConnection.add( setup->lipSyncMixCheck.sigToggled().connect( bind(&BodyMotionGenerationBar::notifyInterpolationParametersChanged, this))); } BodyMotionGenerationBar::~BodyMotionGenerationBar() { delete bodyMotionPoseProvider; delete poseProviderToBodyMotionConverter; } void BodyMotionGenerationBar::notifyInterpolationParametersChanged() { sigInterpolationParametersChanged_.request(); } void BodyMotionGenerationBar::onGenerationButtonClicked() { set motionItems; // for avoiding overlap ItemList selectedItems = ItemTreeView::mainInstance()->selectedItems(); for(size_t i=0; i < selectedItems.size(); ++i){ PoseSeqItem* poseSeqItem = dynamic_cast(selectedItems[i]); if(poseSeqItem){ motionItems.insert(poseSeqItem->bodyMotionItem()); } else { BodyMotionItem* motionItem = dynamic_cast(selectedItems[i]); if(motionItem){ motionItems.insert(motionItem); } } } for(set::iterator p = motionItems.begin(); p != motionItems.end(); ++p){ BodyMotionItem* motionItem = *p; BodyItem* bodyItem = motionItem->findOwnerItem(true); if(bodyItem){ PoseProvider* provider = 0; PoseSeqItem* poseSeqItem = dynamic_cast(motionItem->parentItem()); if(poseSeqItem){ provider = poseSeqItem->interpolator().get(); } else { bodyMotionPoseProvider->initialize(bodyItem->body(), motionItem->motion()); provider = bodyMotionPoseProvider; if(setup->newBodyItemCheck.isChecked()){ BodyMotionItem* newMotionItem = new BodyMotionItem(); newMotionItem->setName(motionItem->name() + "'"); motionItem->parentItem()->insertChildItem(newMotionItem, motionItem->nextItem()); motionItem = newMotionItem; } } shapeBodyMotion(bodyItem->body(), provider, motionItem, true); } } } void BodyMotionGenerationBar::setBalancer(BalancerFunc func, QWidget* panel) { applyBalancer = func; balancerToggle->setEnabled(applyBalancer != 0); balancerPanel = panel; setup->vbox->addWidget(panel); } void BodyMotionGenerationBar::unsetBalancer() { applyBalancer.clear(); balancerToggle->setEnabled(false); setup->layout()->removeWidget(balancerPanel); } bool BodyMotionGenerationBar::shapeBodyMotion (BodyPtr body, PoseProvider* provider, BodyMotionItemPtr motionItem, bool putMessages) { bool result = false; if(balancerToggle->isChecked() && applyBalancer){ result = applyBalancer(body, provider, motionItem, putMessages); } else { result = shapeBodyMotionWithSimpleInterpolation(body, provider, motionItem); } return result; } bool BodyMotionGenerationBar::shapeBodyMotionWithSimpleInterpolation (BodyPtr& body, PoseProvider* provider, BodyMotionItemPtr motionItem) { if(setup->onlyTimeBarRangeCheck.isChecked()){ poseProviderToBodyMotionConverter->setTimeRange(timeBar->minTime(), timeBar->maxTime()); } else { poseProviderToBodyMotionConverter->setFullTimeRange(); } poseProviderToBodyMotionConverter->setAllLinkPositionOutput(setup->se3Check.isChecked()); BodyMotionPtr motion = motionItem->motion(); motion->setFrameRate(timeBar->frameRate()); bool result = poseProviderToBodyMotionConverter->convert(body, provider, *motion); if(result){ motionItem->updateChildItemLineup(); motionItem->notifyUpdate(); } return result; } bool BodyMotionGenerationBar::storeState(Archive& archive) { archive.write("balancer", balancerToggle->isChecked()); archive.write("autoGeneration", autoGenerationToggle->isChecked()); setup->storeState(archive); return true; } bool BodyMotionGenerationBar::restoreState(const Archive& archive) { balancerToggle->setChecked(archive.get("balancer", balancerToggle->isChecked())); autoGenerationToggle->setChecked(archive.get("autoGeneration", autoGenerationToggle->isChecked())); setup->restoreState(archive); return true; } bool BodyMotionGenerationBar::isAutoInterpolationUpdateMode() const { return autoInterpolationUpdateCheck->isChecked(); } bool BodyMotionGenerationBar::isBalancerEnabled() const { return balancerToggle->isChecked(); } bool BodyMotionGenerationBar::isAutoGenerationMode() const { return autoGenerationToggle->isChecked(); } double BodyMotionGenerationBar::timeScaleRatio() const { return setup->timeScaleRatioSpin.value(); } double BodyMotionGenerationBar::preInitialDuration() const { return setup->preInitialDurationSpin.value(); } double BodyMotionGenerationBar::postFinalDuration() const { return setup->postFinalDurationSpin.value(); } bool BodyMotionGenerationBar::isTimeBarRangeOnly() const { return setup->onlyTimeBarRangeCheck.isChecked(); } bool BodyMotionGenerationBar::isStealthyStepMode() const { return setup->stealthyStepCheck.isChecked(); } double BodyMotionGenerationBar::stealthyHeightRatioThresh() const { return setup->stealthyHeightRatioThreshSpin.value(); } double BodyMotionGenerationBar::flatLiftingHeight() const { return setup->flatLiftingHeightSpin.value(); } double BodyMotionGenerationBar::flatLandingHeight() const { return setup->flatLandingHeightSpin.value(); } double BodyMotionGenerationBar::impactReductionHeight() const { return setup->impactReductionHeightSpin.value(); } double BodyMotionGenerationBar::impactReductionTime() const { return setup->impactReductionTimeSpin.value(); } bool BodyMotionGenerationBar::isAutoZmpAdjustmentMode() const { return setup->autoZmpCheck.isChecked(); } double BodyMotionGenerationBar::minZmpTransitionTime() const { return setup->minZmpTransitionTimeSpin.value(); } double BodyMotionGenerationBar::zmpCenteringTimeThresh() const { return setup->zmpCenteringTimeThreshSpin.value(); } double BodyMotionGenerationBar::zmpTimeMarginBeforeLifting() const { return setup->zmpTimeMarginBeforeLiftingSpin.value(); } bool BodyMotionGenerationBar::isSe3Enabled() const { return setup->se3Check.isChecked(); } bool BodyMotionGenerationBar::isLipSyncMixMode() const { return setup->lipSyncMixCheck.isChecked(); } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/BodyMotionGenerationBar.h000066400000000000000000000070021207742442300250070ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_CHOREOGRAPHY_BODY_MOTION_GENERATION_BAR_H_INCLUDED #define CNOID_CHOREOGRAPHY_BODY_MOTION_GENERATION_BAR_H_INCLUDED #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class ExtensionManager; class TimeBar; class PoseProvider; class BodyMotionPoseProvider; class PoseProviderToBodyMotionConverter; class BodyMotionGenerationSetupDialog; class ToggleToolButton; class Action; class CNOID_EXPORT BodyMotionGenerationBar : public ToolBar { public: static void initializeInstance(ExtensionManager* ext); static BodyMotionGenerationBar* instance(); virtual ~BodyMotionGenerationBar(); bool shapeBodyMotion( BodyPtr body, PoseProvider* provider, BodyMotionItemPtr motionItem, bool putMessages = false); typedef boost::function BalancerFunc; void setBalancer(BalancerFunc func, QWidget* panel); void unsetBalancer(); bool isAutoInterpolationUpdateMode() const; bool isBalancerEnabled() const; bool isAutoGenerationMode() const; double timeScaleRatio() const; double preInitialDuration() const; double postFinalDuration() const; double timeToStartBalancer() const; int balancerIterations() const; int boundaryConditionType() const; int boundarySmootherType() const; double boundarySmootherTime() const; double dynamicsTimeRatio() const; bool isTimeBarRangeOnly() const; int initialWaistTrajectoryMode() const; bool isStealthyStepMode() const; double stealthyHeightRatioThresh() const; double flatLiftingHeight() const; double flatLandingHeight() const; double impactReductionHeight() const; double impactReductionTime() const; bool isAutoZmpAdjustmentMode() const; double minZmpTransitionTime() const; double zmpCenteringTimeThresh() const; double zmpTimeMarginBeforeLifting() const; bool isSe3Enabled() const; bool isLipSyncMixMode() const; SignalProxy< boost::signal > sigInterpolationParametersChanged() { return sigInterpolationParametersChanged_.signal(); } private: BodyMotionPoseProvider* bodyMotionPoseProvider; PoseProviderToBodyMotionConverter* poseProviderToBodyMotionConverter; BalancerFunc applyBalancer; QWidget* balancerPanel; TimeBar* timeBar; BodyMotionGenerationSetupDialog* setup; Action* autoInterpolationUpdateCheck; ToolButton* balancerToggle; ToolButton* autoGenerationToggle; LazySignal< boost::signal >sigInterpolationParametersChanged_; ConnectionSet interpolationParameterWidgetsConnection; BodyMotionGenerationBar(); void notifyInterpolationParametersChanged(); void onGenerationButtonClicked(); bool shapeBodyMotionWithSimpleInterpolation (BodyPtr& body, PoseProvider* provider, BodyMotionItemPtr motionItem); virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); }; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/CMakeLists.txt000066400000000000000000000014551207742442300226600ustar00rootroot00000000000000 # @author Shin'ichiro Nakaoka #set(CMAKE_BUILD_TYPE Debug) set(target CnoidPoseSeqPlugin) set(sources PoseSeqPlugin.cpp Pose.cpp PoseSeq.cpp PoseSeqInterpolator.cpp PronunSymbol.cpp PoseFilters.cpp LipSyncTranslator.cpp PoseSeqItem.cpp PoseSeqEngine.cpp PoseSeqViewBase.cpp #PoseSeqView.cpp PoseRollView.cpp BodyMotionGenerationBar.cpp FcpFileLoader.cpp ) set(headers Pose.h PoseSeq.h PoseSeqInterpolator.h PoseFilters.h PoseSeqItem.h BodyMotionGenerationBar.h exportdecl.h gettext.h ) QT4_ADD_RESOURCES(RC_SRCS PoseSeqPlugin.qrc) make_gettext_mofiles(mofiles) add_library(${target} SHARED ${sources} ${headers} ${mofiles} ${RC_SRCS}) target_link_libraries(${target} CnoidUtil CnoidBody CnoidBodyPlugin) apply_common_setting_for_plugin(${target} "${headers}") choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/FcpFileLoader.cpp000066400000000000000000000244011207742442300232570ustar00rootroot00000000000000/** @file FaceController Plugin File (*.poseset and *.poseseq) Loader @author Shin'ichiro Nakaoka */ #include "FcpFileLoader.h" #include "PoseSeqItem.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { inline double radian(double deg) { return (3.14159265358979 * deg / 180.0); } struct FcPose { vector q; double transitionTime; }; struct Part { vector jointIds; map poses; }; typedef boost::shared_ptr PartPtr; vector parts; bool loadFaceControllerPoseSet(const string& filename) { ostream& os = MessageView::mainInstance()->cout(); parts.clear(); ifstream ifs(filename.c_str()); if(!ifs.is_open()){ os << filename << " is not found.\n"; return false; } os << "Loading " << filename << "..." << endl; int nLines = 0; try { typedef tokenizer< char_separator > tokenizer; char_separator sep(" \t\r\n"); string line; while(getline(ifs, line)){ ++nLines; // for a text with CRLF if(!line.empty() && line[line.length()-1] == '\r'){ line = line.substr(0, line.length()-1); } tokenizer tokens(line, sep); tokenizer::iterator it = tokens.begin(); if(it != tokens.end()){ if(*it == "*"){ ++it; PartPtr part(new Part()); while(it != tokens.end()){ part->jointIds.push_back(lexical_cast(*it++)); } if(!part->jointIds.empty()){ os << "pose registration of the joint set "; size_t i=0; while(true){ os << part->jointIds[i++]; if(i == part->jointIds.size()){ break; } os << ", "; } os << endl; parts.push_back(part); } } else if(parts.empty()){ os << "Poses cannot be defined without a part definition"; os << " at line " << nLines << "." << endl; break; } else { PartPtr part = parts.back(); string label(*it++); FcPose pose; if(it != tokens.end()){ while(true){ double value = lexical_cast(*it++); if(it == tokens.end()){ pose.transitionTime = value; break; } pose.q.push_back(radian(value)); } } if(pose.q.size() != part->jointIds.size()){ os << "label \"" << label << "\" is not correctly defined"; os << " at line " << nLines << "." << endl; } else { part->poses[label] = pose; os << label << " "; for(size_t i=0; i < pose.q.size(); ++i){ os << pose.q[i] << ", "; } os << " : " << pose.transitionTime; os << endl; } } } } if(!parts.empty()){ cout << endl; } } catch(const bad_lexical_cast& ex){ os << ex.what() << " at line " << nLines << endl; parts.clear(); } return !parts.empty(); } PoseSeqItemPtr loadFaceControllerPoseSeq(const string& filename) { ostream& os = MessageView::mainInstance()->cout(); ifstream ifs(filename.c_str()); if(!ifs.is_open()){ os << filename + "is not found" << endl; return 0; } os << "Loading " << filename << "..." << endl; PoseSeqItemPtr item = new PoseSeqItem(); filesystem::path fpath(filename); item->setName(basename(fpath)); PoseSeqPtr seq = item->poseSeq(); int nLines = 0; try { typedef tokenizer< char_separator > tokenizer; char_separator sep(" ,\t\r\n", "", boost::keep_empty_tokens); int nPoses = 0; string line; while(getline(ifs, line)){ nLines++; // for a text with CRLF if(!line.empty() && line[line.length()-1] == '\r'){ line = line.substr(0, line.length()-1); } tokenizer tokens(line, sep); tokenizer::iterator it = tokens.begin(); if(it != tokens.end()){ PoseSeq::iterator poseIter = seq->begin(); double time = lexical_cast(*it++); int numParts = parts.size(); bool poseAdded = false; for(int i=0; it != tokens.end(); ++i, ++it){ if(i == numParts){ os << "line at time " << time << " contains parts more than defined ones." << endl; break; } Part& part = *parts[i]; string label(*it); if(!label.empty()){ map::iterator p = part.poses.find(label); if(p == part.poses.end()){ os << "label \"" << label << "\" is not defined"; os << " at line " << nLines << "." << endl; } else { const FcPose& fcPose = p->second; PosePtr pose(new Pose()); for(size_t j=0; j < part.jointIds.size(); ++j){ pose->setJointPosition(part.jointIds[j], fcPose.q[j]); } poseIter = seq->insert(poseIter, time, pose); poseIter->setMaxTransitionTime(fcPose.transitionTime); poseAdded = true; } } } if(poseAdded){ nPoses++; } } } os << "A pose sequence with " << nPoses << " poses has been loaded." << endl; } catch(const bad_lexical_cast& ex){ os << "FaceController : " << ex.what() << endl; os << " at line " << nLines << "." << endl; item = 0; } return item; } void invokeFaceControllerPatternFileImportDialog() { /// \todo The function by the following code shoulde be provided by ItemManger QFileDialog dialog(MainWindow::instance()); dialog.setWindowTitle(_("Choose poseset file")); dialog.setFileMode(QFileDialog::ExistingFile); dialog.setViewMode(QFileDialog::List); dialog.setLabelText(QFileDialog::Accept, _("Open")); dialog.setLabelText(QFileDialog::Reject, _("Cancel")); QStringList filters; filters << _("FaceController poseset files (*.poseset)"); filters << _("Any files (*)"); dialog.setNameFilters(filters); string currentFolder; if(AppConfig::archive()->read("currentFileDialogDirectory", currentFolder)){ dialog.setDirectory(currentFolder.c_str()); } string posesetFile; if(dialog.exec()){ posesetFile = dialog.selectedFiles().front().toStdString(); dialog.setWindowTitle(_("Choose poseseq files")); QStringList filters; filters << _("FaceController poseseq files (*.poseseq)"); filters << _("Any files (*)"); dialog.setNameFilters(filters); if(dialog.exec()){ dialog.hide(); MessageView::mainInstance()->flush(); ItemTreeView* itv = ItemTreeView::mainInstance(); Item* parentItem = itv->selectedItem(); if(!parentItem){ parentItem = itv->rootItem(); } QStringList poseseqFiles = dialog.selectedFiles(); AppConfig::archive()->write( "currentFileDialogDirectory", dialog.directory().absolutePath().toStdString(), YAML_DOUBLE_QUOTED); if(loadFaceControllerPoseSet(posesetFile)){ for(int i=0; i < poseseqFiles.size(); ++i){ PoseSeqItemPtr item = loadFaceControllerPoseSeq(poseseqFiles[i].toStdString()); if(item){ parentItem->addChildItem(item); } } } } } dialog.hide(); } } void cnoid::initializeFcpFileLoader(ExtensionManager& ext) { MenuManager& mm = ext.menuManager(); mm.setPath("/File/Import ..."); mm.addItem(_("FaceController Plugin Pattern Files")) ->sigTriggered().connect(invokeFaceControllerPatternFileImportDialog); } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/FcpFileLoader.h000066400000000000000000000004421207742442300227230ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_CHOREOGRAPHY_PLUGIN_FCP_FILE_LOADER_H_INCLUDED #define CNOID_CHOREOGRAPHY_PLUGIN_FCP_FILE_LOADER_H_INCLUDED namespace cnoid { class ExtensionManager; void initializeFcpFileLoader(ExtensionManager& ext); } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/LipSyncTranslator.cpp000066400000000000000000000063521207742442300242600ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "LipSyncTranslator.h" #include "PronunSymbol.h" #include "PoseSeq.h" #include using namespace std; using namespace boost; using namespace cnoid; LipSyncTranslator::LipSyncTranslator() { isMaxTransitionTimeEnabled_ = false; maxTransitionTime_ = 0.2; } void LipSyncTranslator::clear() { seq.clear(); } void LipSyncTranslator::translatePoseSeq(PoseSeq& poseSeq) { clear(); for(PoseSeq::iterator poseIter = poseSeq.begin(); poseIter != poseSeq.end(); ++poseIter){ PronunSymbolPtr symbol = poseIter->get(); if(symbol && !symbol->name().empty()){ appendSyllable(poseIter->time(), symbol->name()); } } } bool LipSyncTranslator::appendSyllable(double time, const std::string& syllable) { if(syllable.empty()){ return false; } int vowel = -1; switch(tolower(syllable[syllable.size()-1])){ case 'a': vowel = LS_A; break; case 'i': vowel = LS_I; break; case 'u': vowel = LS_U; break; case 'e': vowel = LS_E; break; case 'o': vowel = LS_O; break; case 'n': vowel = LS_N; break; case ',': vowel = LS_N; break; case '.': vowel = LS_N; break; default: break; } if(vowel < 0){ return false; } Phoneme phoneme0; phoneme0.shapeId = -1; Phoneme phoneme1; phoneme1.shapeId = -1; if(vowel != LS_N && syllable.size() >= 2){ int consonantChar = tolower(syllable[0]); if(consonantChar == 'm' || consonantChar == 'b' || consonantChar == 'p'){ phoneme0.shapeId = LS_N; } else if(!seq.empty()){ int prevVowel = seq.back().shapeId; if(vowel == prevVowel){ phoneme0.shapeId = vowel + LS_a; } } } if(phoneme0.shapeId < 0){ phoneme0.shapeId = vowel; } else { phoneme1.shapeId = vowel; } while(!seq.empty()){ double prevTime = seq.back().time; double ttime = time - prevTime; if(ttime <= 0.0){ seq.pop_back(); continue; } if(isMaxTransitionTimeEnabled_ && (ttime > maxTransitionTime_)){ seq.push_back(seq.back()); seq.back().time = time - maxTransitionTime_; } break; } phoneme0.time = time; seq.push_back(phoneme0); if(phoneme1.shapeId >= 0){ phoneme1.time = time + 0.05; seq.push_back(phoneme1); } return true; } bool LipSyncTranslator::exportSeqFileForFaceController(const std::string& filename) { ofstream ofs(filename.c_str()); for(iterator p = seq.begin(); p != seq.end(); ++p){ ofs << p->time << ", "; switch (p->shapeId) { case LS_A: ofs << "a"; break; case LS_a: ofs << "a0"; break; case LS_I: ofs << "i"; break; case LS_i: ofs << "i0"; break; case LS_U: ofs << "u"; break; case LS_u: ofs << "u0"; break; case LS_E: ofs << "e"; break; case LS_e: ofs << "e0"; break; case LS_O: ofs << "o"; break; case LS_o: ofs << "o0"; break; case LS_N: ofs << "n"; break; default: break; } ofs << "\n"; } ofs.close(); return true; } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/LipSyncTranslator.h000066400000000000000000000037761207742442300237340ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_CHOREOGRAPHY_LIP_SYNC_TRANSLATOR_H_INCLUDED #define CNOID_CHOREOGRAPHY_LIP_SYNC_TRANSLATOR_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class PoseSeq; class CNOID_EXPORT LipSyncTranslator { public: enum LipShapeId { LS_A, LS_I, LS_U, LS_E, LS_O, LS_N, LS_a, LS_i, LS_u, LS_e, LS_o, NUM_LIP_SHAPES }; struct Phoneme { double time; int shapeId; }; typedef std::list::iterator iterator; typedef std::list::const_iterator const_iterator; LipSyncTranslator(); void translatePoseSeq(PoseSeq& poseSeq); bool appendSyllable(double time, const std::string& syllable); bool exportSeqFileForFaceController(const std::string& filename); void enableMaxTransitionTime(bool on){ isMaxTransitionTimeEnabled_ = on; } bool isMaxTransitionTimeEnabled() const { return isMaxTransitionTimeEnabled_; } void setMaxTransitionTime(double ttime) { maxTransitionTime_ = ttime; } double maxTransitionTime() const { return maxTransitionTime_; } void clear(); inline bool empty() const { return seq.empty(); } inline std::list::size_type size() const { return seq.size(); } inline iterator begin(){ return seq.begin(); } inline const_iterator begin() const { return seq.begin(); } inline iterator end(){ return seq.end(); } inline const_iterator end() const { return seq.end(); } private: typedef std::list PhonemeList; PhonemeList seq; bool isMaxTransitionTimeEnabled_; double maxTransitionTime_; }; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/Pose.cpp000066400000000000000000000166251207742442300215370ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "Pose.h" #include #include #include using namespace std; using namespace boost; using namespace cnoid; PoseUnit::PoseUnit() { owner = 0; seqLocalReferenceCounter = 0; } PoseUnit::PoseUnit(const PoseUnit& org) : name_(org.name_) { owner = 0; seqLocalReferenceCounter = 0; } PoseUnit::~PoseUnit() { } Pose::Pose() { initializeMembers(); } Pose::Pose(int numJoints) : jointInfos(numJoints) { initializeMembers(); } Pose::Pose(const Pose& org) : PoseUnit(org), jointInfos(org.jointInfos), ikLinks(org.ikLinks) { zmp_ = org.zmp_; isZmpValid_ = org.isZmpValid_; isZmpStationaryPoint_ = org.isZmpStationaryPoint_; baseLinkIter = ikLinks.end(); if(org.baseLinkIter != org.ikLinks.end()){ int baseLinkIndex = org.baseLinkIter->first; baseLinkIter = ikLinks.find(baseLinkIndex); } } void Pose::initializeMembers() { baseLinkIter = ikLinks.end(); isZmpValid_ = false; isZmpStationaryPoint_ = true; } Pose::~Pose() { } bool Pose::hasSameParts(PoseUnitPtr unit) { PosePtr pose = dynamic_pointer_cast(unit); if(!pose){ return false; } const int n = numJoints(); if(n != pose->numJoints()){ return false; } for(int i=0; i < n; ++i){ if(isJointValid(i) != pose->isJointValid(i)){ return false; } } return true; } bool Pose::empty() { if(!ikLinks.empty()){ return false; } if(isZmpValid_){ return false; } for(size_t i=0; i < jointInfos.size(); ++i){ if(jointInfos[i].isValid){ return false; } } return true; } void Pose::clear() { jointInfos.clear(); ikLinks.clear(); initializeMembers(); } void Pose::clearIkLinks() { ikLinks.clear(); baseLinkIter = ikLinks.end(); } bool Pose::removeIkLink(int linkIndex) { LinkInfoMap::iterator p = ikLinks.find(linkIndex); if(p != ikLinks.end()){ if(p == baseLinkIter){ baseLinkIter = ikLinks.end(); } ikLinks.erase(p); return true; } return false; } Pose::LinkInfo& Pose::setBaseLink(int linkIndex) { if(baseLinkIter != ikLinks.end()){ const int oldIndex = baseLinkIter->first; if(linkIndex == oldIndex){ return baseLinkIter->second; } baseLinkIter->second.isBaseLink_ = false; } baseLinkIter = ikLinks.insert(make_pair(linkIndex, LinkInfo())).first; LinkInfo& info = baseLinkIter->second; info.isBaseLink_ = true; return info; } PoseUnit* Pose::duplicate() { return new Pose(*this); } bool Pose::restore(const YamlMapping& archive, const BodyPtr body) { clear(); const YamlSequence& jointIndices = *archive.findSequence("joints"); if(jointIndices.isValid()){ int maxIndex = jointIndices.back().toInt(); setNumJoints(maxIndex + 1); const YamlSequence& qs = *archive["q"].toSequence(); for(int i=0; i < jointIndices.size(); ++i){ setJointPosition(jointIndices[i].toInt(), qs[i].toDouble()); } } const YamlSequence& stationaryPoints = *archive.findSequence("spJoints"); if(stationaryPoints.isValid()){ for(int i=0; i < stationaryPoints.size(); ++i){ jointInfos[stationaryPoints[i].toInt()].isStationaryPoint = true; } } const YamlSequence& ikLinkNodes = *archive.findSequence("ikLinks"); if(ikLinkNodes.isValid()){ for(int i=0; i < ikLinkNodes.size(); ++i){ const YamlMapping& ikLinkNode = *ikLinkNodes[i].toMapping(); int index = -1; YamlNode* nameNode = ikLinkNode.find("name"); if(nameNode->isValid()){ Link* link = body->link(nameNode->toString()); if(link){ index = link->index; } } if(index < 0){ YamlNode* indexNode = ikLinkNode.find("index"); if(indexNode->isValid()){ index = indexNode->toInt(); } } if(index >= 0){ Vector3 p; Matrix3 R; if(read(ikLinkNode, "translation", p) && read(ikLinkNode, "rotation", R)){ LinkInfo* info = addIkLink(index); info->p = p; info->R = R; info->setStationaryPoint(ikLinkNode.get("isStationaryPoint", false)); if(ikLinkNode.get("isBaseLink", false)){ setBaseLink(index); } Vector3 partingDirection; if(ikLinkNode.get("isTouching", false) && read(ikLinkNode, "partingDirection", partingDirection)){ info->setTouching(partingDirection); } info->setSlave(ikLinkNode.get("isSlave", false)); } } } } if(read(archive, "zmp", zmp_)){ isZmpValid_ = true; archive.read("isZmpStationaryPoint", isZmpStationaryPoint_); } return true; } void Pose::store(YamlMapping& archive, const BodyPtr body) const { archive.write("type", "Pose"); archive.write("name", name(), YAML_DOUBLE_QUOTED); YamlSequencePtr jointIndices = new YamlSequence(); YamlSequencePtr qs = new YamlSequence(); qs->setDoubleFormat(archive.doubleFormat()); YamlSequencePtr spJoints = new YamlSequence(); int n = numJoints(); for(int i=0; i < n; ++i){ const JointInfo& info = jointInfos[i]; if(info.isValid){ jointIndices->append(i, 10, n); qs->append(info.q, 10, n); if(info.isStationaryPoint){ spJoints->append(i, 10); } } } if(!jointIndices->empty()){ jointIndices->setFlowStyle(); archive.insert("joints", jointIndices); qs->setFlowStyle(); archive.insert("q", qs); if(!spJoints->empty()){ spJoints->setFlowStyle(); archive.insert("spJoints", spJoints); } } if(!ikLinks.empty()){ YamlSequence& ikLinkNodes = *archive.createSequence("ikLinks"); for(LinkInfoMap::const_iterator p = ikLinks.begin(); p != ikLinks.end(); ++p){ const int index = p->first; const LinkInfo& info = p->second; YamlMapping& ikLinkNode = *ikLinkNodes.newMapping(); ikLinkNode.write("name", body->link(index)->name()); ikLinkNode.write("index", index); if(info.isBaseLink()){ ikLinkNode.write("isBaseLink", info.isBaseLink()); } if(info.isStationaryPoint()){ ikLinkNode.write("isStationaryPoint", info.isStationaryPoint()); } write(ikLinkNode, "translation", info.p); write(ikLinkNode, "rotation", info.R); if(info.isTouching()){ ikLinkNode.write("isTouching", true); write(ikLinkNode, "partingDirection", info.partingDirection()); } if(info.isSlave()){ ikLinkNode.write("isSlave", true); } } } if(isZmpValid()){ write(archive, "zmp", zmp_); archive.write("isZmpStationaryPoint", isZmpStationaryPoint_); } } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/Pose.h000066400000000000000000000165471207742442300212070ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_CHOREOGRAPHY_POSE_H_INCLUDED #define CNOID_CHOREOGRAPHY_POSE_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { class YamlMapping; class PoseUnit; class PoseSeq; class PoseRef; typedef boost::intrusive_ptr PoseUnitPtr; class CNOID_EXPORT PoseUnit : public Referenced { public: PoseUnit(); PoseUnit(const PoseUnit& org); virtual ~PoseUnit(); virtual PoseUnit* duplicate() = 0; virtual bool restore(const YamlMapping& archive, const BodyPtr body) = 0; virtual void store(YamlMapping& archive, const BodyPtr body) const = 0; virtual bool hasSameParts(PoseUnitPtr unit) { return false; } /** @note A name can be only set by PoseSeq::rename(). */ inline const std::string& name() const { return name_; } private: std::string name_; PoseSeq* owner; int seqLocalReferenceCounter; friend class PoseSeq; friend class PoseRef; }; class CNOID_EXPORT Pose : public PoseUnit { struct JointInfo { inline JointInfo() : isValid(false), isStationaryPoint(false) { } double q; bool isValid; bool isStationaryPoint; }; public: class LinkInfo { public: Vector3 p; Matrix3 R; inline LinkInfo() : isBaseLink_(false), isStationaryPoint_(false), isTouching_(false), isSlave_(false) { } inline bool isBaseLink() const { return isBaseLink_; } inline void setStationaryPoint(bool on){ isStationaryPoint_ = on; } inline bool isStationaryPoint() const { return isStationaryPoint_; } inline bool isTouching() const { return isTouching_; } inline const Vector3& partingDirection() const { return partingDirection_; } inline void setTouching(const Vector3& partingDirection) { isTouching_ = true; partingDirection_ = partingDirection; } inline void clearTouching() { isTouching_ = false; } inline bool isSlave() const { return isSlave_; } inline void setSlave(bool on) { isSlave_ = on; } private: bool isBaseLink_; bool isStationaryPoint_; bool isTouching_; bool isSlave_; Vector3 partingDirection_; friend class Pose; }; typedef std::map LinkInfoMap; Pose(); Pose(int numJoints); Pose(const Pose& org); virtual ~Pose(); bool empty(); void clear(); virtual PoseUnit* duplicate(); virtual bool hasSameParts(PoseUnitPtr unit); virtual bool restore(const YamlMapping& archive, const BodyPtr body); virtual void store(YamlMapping& archive, const BodyPtr body) const; inline void setNumJoints(int n){ jointInfos.resize(n); } inline int numJoints() const { return jointInfos.size(); } inline void setJointPosition(int jointId, double q){ if(jointId >= (int)jointInfos.size()){ setNumJoints(jointId + 1); } JointInfo& info = jointInfos[jointId]; info.q = q; info.isValid = true; } inline double jointPosition(int jointId) const { return jointInfos[jointId].q; } inline bool isJointValid(int jointId) const { if(jointId < 0 || jointId >= (int)jointInfos.size()){ return false; } return jointInfos[jointId].isValid; } inline void setJointStationaryPoint(int jointId, bool on = true){ if(jointId >= (int)jointInfos.size()){ setNumJoints(jointId + 1); } jointInfos[jointId].isStationaryPoint = on; } inline bool isJointStationaryPoint(int jointId) const { if(jointId >= (int)jointInfos.size()){ return false; } return jointInfos[jointId].isStationaryPoint; } inline bool invalidateJoint(int jointId) { if(jointId < (int)jointInfos.size()){ if(jointInfos[jointId].isValid){ jointInfos[jointId].isValid = false; return true; } } return false; } void clearIkLinks(); inline size_t numIkLinks(){ return ikLinks.size(); } inline LinkInfo* addIkLink(int linkIndex){ return &ikLinks[linkIndex]; } bool removeIkLink(int linkIndex); inline const LinkInfo* ikLinkInfo(int linkIndex) const { LinkInfoMap::const_iterator p = ikLinks.find(linkIndex); return (p != ikLinks.end()) ? &p->second : 0; } inline LinkInfo* ikLinkInfo(int linkIndex) { LinkInfoMap::iterator p = ikLinks.find(linkIndex); return (p != ikLinks.end()) ? &p->second : 0; } inline LinkInfoMap::iterator ikLinkBegin() { return ikLinks.begin(); } inline const LinkInfoMap::const_iterator ikLinkBegin() const { return ikLinks.begin(); } inline LinkInfoMap::iterator ikLinkEnd() { return ikLinks.end(); } inline const LinkInfoMap::const_iterator ikLinkEnd() const { return ikLinks.end(); } LinkInfo& setBaseLink(int linkIndex); inline LinkInfo& setBaseLink(int linkIndex, const Vector3& p, const Matrix3& R){ LinkInfo& info = setBaseLink(linkIndex); info.p = p; info.R = R; return info; } inline int baseLinkIndex() const { return (baseLinkIter != ikLinks.end()) ? baseLinkIter->first : -1; } inline LinkInfo* baseLinkInfo() { return (baseLinkIter != ikLinks.end()) ? &baseLinkIter->second : 0; } void invalidateBaseLink() { if(baseLinkIter != ikLinks.end()){ baseLinkIter->second.isBaseLink_ = false; baseLinkIter = ikLinks.end(); } } inline void setZmp(const Vector3& p){ isZmpValid_ = true; zmp_ = p; } inline const Vector3 zmp() const { return zmp_; } inline bool isZmpValid() const { return isZmpValid_; } inline bool invalidateZmp() { bool ret = isZmpValid_; isZmpValid_ = false; return ret; } inline void setZmpStationaryPoint(bool on = true){ isZmpStationaryPoint_ = on; } inline bool isZmpStationaryPoint() const { return isZmpStationaryPoint_; } private: std::vector jointInfos; LinkInfoMap ikLinks; LinkInfoMap::iterator baseLinkIter; Vector3 zmp_; bool isZmpValid_; bool isZmpStationaryPoint_; void initializeMembers(); }; typedef boost::intrusive_ptr PosePtr; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseFilters.cpp000066400000000000000000000260741207742442300230670ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "PoseFilters.h" #include using namespace std; using namespace cnoid; namespace { class StepAdjuster { public: PoseSeqPtr seq; const vector& footLinkIndices; map supportingLinks; Vector3 stepAdjustmentTranslation; double stepAdjustmentYawDiff; Matrix3 stepAdjustmentRotation; StepAdjuster(PoseSeqPtr seq, const vector& footLinkIndices, PoseSeq::iterator origin) : seq(seq), footLinkIndices(footLinkIndices) { supportingLinks.clear(); stepAdjustmentTranslation.setZero(); stepAdjustmentYawDiff = 0.0; stepAdjustmentRotation.setIdentity(); PoseSeq::iterator poseIter; for(poseIter = origin; poseIter != seq->end(); ++poseIter){ adjustStepPosition(poseIter); } supportingLinks.clear(); stepAdjustmentTranslation.setZero(); stepAdjustmentYawDiff = 0.0; stepAdjustmentRotation.setIdentity(); poseIter = origin; while(true){ adjustStepPosition(poseIter); if(poseIter == seq->begin()){ break; } poseIter--; } } void adjustStepPosition(PoseSeq::iterator poseIter) { PosePtr pose = poseIter->get(); if(pose){ seq->beginPoseModification(poseIter); bool modified = false; Vector3 dp = Vector3::Zero(); double da = 0.0; for(size_t i=0; i < footLinkIndices.size(); ++i){ int linkIndex = footLinkIndices[i]; Pose::LinkInfo* info = pose->ikLinkInfo(linkIndex); if(info){ map::iterator p = supportingLinks.find(linkIndex); if(p != supportingLinks.end()){ if(!info->isTouching()){ supportingLinks.erase(p); } else { Pose::LinkInfo* prev = p->second; if(prev->p != info->p){ dp += prev->p - info->p; info->p = prev->p; modified = true; } if(prev->R != info->R){ const Matrix3 R = info->R.transpose() * prev->R; da += atan2(R(1,0),R(0,0)); info->R = prev->R; modified = true; } p->second = info; } } } } double ns = supportingLinks.size(); if(modified && ns > 0){ stepAdjustmentTranslation[0] = dp[0] / ns; stepAdjustmentTranslation[1] = dp[1] / ns; stepAdjustmentYawDiff = da / ns; stepAdjustmentRotation = AngleAxisd(stepAdjustmentYawDiff, Vector3::UnitZ()); } bool isDifferent = (stepAdjustmentTranslation != Vector3::Zero() || stepAdjustmentYawDiff != 0.0); if(isDifferent){ modified = true; } for(Pose::LinkInfoMap::iterator it = pose->ikLinkBegin(); it != pose->ikLinkEnd(); ++it){ int linkIndex = it->first; if(supportingLinks.find(linkIndex) == supportingLinks.end()){ Pose::LinkInfo& info = it->second; if(isDifferent){ info.p += stepAdjustmentTranslation; info.R = stepAdjustmentRotation * info.R; } if(info.isTouching()){ supportingLinks.insert(make_pair(linkIndex, &info)); } } } if(pose->isZmpValid() && isDifferent){ pose->setZmp(pose->zmp() + stepAdjustmentTranslation); } if(modified){ seq->endPoseModification(poseIter); } } } }; } void cnoid::adjustStepPositions(PoseSeqPtr seq, const vector& footLinkIndices, PoseSeq::iterator origin) { StepAdjuster adjuster(seq, footLinkIndices, origin); } namespace { struct JointFlipInfo { int counterPartJointId; double jointPositionSign; }; class FlipFilter { BodyPtr body; typedef map JointFlipInfoMap; JointFlipInfoMap jointFlipInfoMap; typedef map LinkFlipMap; LinkFlipMap linkFlipMap; bool flipPose(PosePtr pose); public: FlipFilter(BodyPtr body); void flip(PoseSeqPtr seq); }; } FlipFilter::FlipFilter(BodyPtr body) : body(body) { const YamlSequence& sjoints = *body->info()->findSequence("symmetricJoints"); if(sjoints.isValid() && !sjoints.empty()){ for(int i=0; i < sjoints.size(); ++i){ const YamlSequence& jointPair = *sjoints[i].toSequence(); if(jointPair.size() == 1){ Link* link = body->link(jointPair[0].toString()); if(link){ JointFlipInfo& info = jointFlipInfoMap[link->jointId]; info.counterPartJointId = link->jointId; info.jointPositionSign = -1.0; } } else if(jointPair.size() >= 2){ Link* joint[2]; joint[0] = body->link(jointPair[0].toString()); joint[1] = body->link(jointPair[1].toString()); if(joint[0] && joint[1] && joint[0]->jointId >= 0 && joint[1]->jointId >= 0){ for(int j=0; j < 2; ++j){ int other = 1 - j; JointFlipInfo& info = jointFlipInfoMap[joint[j]->jointId]; info.counterPartJointId = joint[other]->jointId; info.jointPositionSign = 1.0; if(jointPair.size() >= 3){ info.jointPositionSign = jointPair[2].toDouble(); } } } } } } const YamlSequence& slinks = *body->info()->findSequence("symmetricIkLinks"); if(slinks.isValid() && !slinks.empty()){ for(int i=0; i < slinks.size(); ++i){ const YamlSequence& linkPair = *slinks[i].toSequence(); if(linkPair.size() == 1){ Link* link = body->link(linkPair[0].toString()); if(link){ linkFlipMap[link->index] = link->index; } } else if(linkPair.size() >= 2){ Link* link[2]; link[0] = body->link(linkPair[0].toString()); link[1] = body->link(linkPair[1].toString()); if(link[0] && link[1]){ for(int j=0; j < 2; ++j){ int other = 1 - j; linkFlipMap[link[j]->index] = link[other]->index; } } } } } } void FlipFilter::flip(PoseSeqPtr seq) { PoseSeq::iterator poseIter; for(poseIter = seq->begin(); poseIter != seq->end(); ++poseIter){ PosePtr pose = poseIter->get(); if(pose){ seq->beginPoseModification(poseIter); if(flipPose(pose)){ seq->endPoseModification(poseIter); } } } } bool FlipFilter::flipPose(PosePtr pose) { bool modified = false; PosePtr orgPose = static_cast(pose->duplicate()); pose->setNumJoints(0); for(int i=0; i < orgPose->numJoints(); ++i){ if(orgPose->isJointValid(i)){ double q = orgPose->jointPosition(i); JointFlipInfoMap::iterator p = jointFlipInfoMap.find(i); if(p == jointFlipInfoMap.end()){ pose->setJointPosition(i, q); } else { JointFlipInfo& flip = p->second; pose->setJointPosition(flip.counterPartJointId, q * flip.jointPositionSign); modified = true; } } } pose->clearIkLinks(); for(Pose::LinkInfoMap::iterator p = orgPose->ikLinkBegin(); p != orgPose->ikLinkEnd(); ++p){ int index = p->first; Pose::LinkInfo& orgInfo = p->second; Pose::LinkInfo* info; LinkFlipMap::iterator q = linkFlipMap.find(index); if(q != linkFlipMap.end()){ index = q->second; info = pose->addIkLink(index); info->p = orgInfo.p; info->p.y() = -info->p.y(); Matrix3& R = orgInfo.R; info->R << R(0, 0), -R(0, 1), R(0, 2), -R(1, 0), R(1, 1), -R(1, 2), R(2, 0), -R(2, 1), R(2, 2); modified = true; } else { info = pose->addIkLink(index); info->p = orgInfo.p; info->R = orgInfo.R; } info->setStationaryPoint(orgInfo.isStationaryPoint()); if(orgInfo.isTouching()){ info->setTouching(orgInfo.partingDirection()); } info->setSlave(orgInfo.isSlave()); if(orgInfo.isBaseLink()){ pose->setBaseLink(index); } } if(orgPose->isZmpValid()){ Vector3 zmp = orgPose->zmp(); zmp.y() = -zmp.y(); pose->setZmp(zmp); pose->setZmpStationaryPoint(orgPose->isZmpStationaryPoint()); modified = true; } return modified; } void cnoid::flipPoses(PoseSeqPtr seq, BodyPtr body) { FlipFilter filiter(body); filiter.flip(seq); } void cnoid::rotateYawOrientations (PoseSeqPtr seq, PoseSeq::iterator begin, const Vector3& center, double angle) { const Matrix3 Rz(AngleAxisd(angle, Vector3::UnitZ())); PoseSeq::iterator poseIter; for(poseIter = begin; poseIter != seq->end(); ++poseIter){ PosePtr pose = poseIter->get(); if(pose){ if(pose->numIkLinks() > 0 || pose->isZmpValid()){ seq->beginPoseModification(poseIter); for(Pose::LinkInfoMap::iterator p = pose->ikLinkBegin(); p != pose->ikLinkEnd(); ++p){ Pose::LinkInfo& linkInfo = p->second; linkInfo.p = Rz * (linkInfo.p - center) + center; linkInfo.R = Rz * linkInfo.R; } if(pose->isZmpValid()){ pose->setZmp(Rz * (pose->zmp() - center) + center); } seq->endPoseModification(poseIter); } } } } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseFilters.h000066400000000000000000000013061207742442300225230ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_CHOREOGRAPHY_POSE_FILTERS_H_INCLUDED #define CNOID_CHOREOGRAPHY_POSE_FILTERS_H_INCLUDED #include "PoseSeq.h" #include "exportdecl.h" namespace cnoid { CNOID_EXPORT void adjustStepPositions( PoseSeqPtr seq, const std::vector& footLinkIndices, PoseSeq::iterator origin); CNOID_EXPORT void flipPoses(PoseSeqPtr seq, BodyPtr body); CNOID_EXPORT void adjustWaistHeight( PosePtr pose, int waistLinkIndex, const std::vector& footLinkIndices, double offset); CNOID_EXPORT void rotateYawOrientations( PoseSeqPtr seq, PoseSeq::iterator begin, const Vector3& center, double angle); } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseRollView.cpp000066400000000000000000001242651207742442300232230ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "PoseRollView.h" #include "PoseSeqViewBase.h" #include "PronunSymbol.h" #include #include #include #include #include #include #include #include #include #include "gettext.h" #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; const bool FAST_DRAW_MODE = false; const double leftMargin = 0.2; class ScrollBarEx : public ScrollBar { public: ScrollBarEx(Qt::Orientation orientation) : ScrollBar(orientation) { } virtual void sliderChange(SliderChange change){ ScrollBar::sliderChange(change); if(change == QAbstractSlider::SliderStepsChange){ if((maximum() - minimum()) == 0){ hide(); } else { show(); } } } }; } namespace cnoid { class PoseRollViewImpl : public PoseSeqViewBase { public: PoseRollViewImpl(PoseRollView* self); ~PoseRollViewImpl(); PoseRollView* self; QGridLayout* gridLayout; ScrollBarEx* treeWidgetVerticalScrollBar; QWidget* screen; QVector dash; QPainter painter; QPen markerPen; QPen pronumSymbolPen; QPen selectedMarkerPen; QPen gridPen; QPen cursorPen; ToolButton commandMenuButton; Menu commandMenu; MenuManager commandMenuManager; Action* lipSyncCheck; QLabel poseNameLabel; DoubleSpinBox currentTimeSpin; signals::connection currentTimeSpinConnection; DoubleSpinBox poseTimeSpin; signals::connection poseTimeSpinConnection; DoubleSpinBox poseTTimeSpin; signals::connection poseTTimeSpinConnection; DoubleScrollBar* hScrollBar; signals::connection hScrollBarChangedConnection; bool isTmpScrollBlocked; double left; double right; double treeTopOnScreen; double screenWidth; double screenHeight; double treeBottomOnScreen; double timeToScreenX; double gridStep; double timeLength; DoubleSpinBox timeLengthSpin; DoubleSpinBox gridIntervalSpin; bool updateRowRectsNeeded; struct RowInfo { RowInfo() { } bool isVisible; int visibleRowIndex; int y; int height; int linkIndex; int jointId; }; vector itemIndexToRowInfoMap; vector visibleRows; vector linkIndexToVisibleRowAncestorMap; LinkTreeItem* visibleRowAncestorOfZmp; struct RowRenderInfo { bool rendered; double lastPoseTime; }; vector rowRenderInfos; double pointerX; double pointerY; double pressedScreenX; double dragOrgLeft; double dragOrgScale; double pickDistance; PoseSeq::iterator pickedPoseIter; enum PickedPart { PICK_NONE, PICK_LEFT, PICK_BODY, PICK_RIGHT }; PickedPart pickedPart; double pickedTime; enum DragMode { DRAG_NONE, DRAG_POSES, DRAG_TRANSITION_TIME, DRAG_TIME_CURSOR, DRAG_SCALING } dragMode; enum DragState { DS_INITIAL, DS_DRAGGED } dragState; double draggingPosesOrgTime; // for key pose marker rendering PoseSeq::iterator markerPoseIter; double markerTime0; double markerX0; double markerX1; double markerY0; double markerY1; bool isMarkerPronunSymbol; bool isMarkerSelected; void initialize(); QHBoxLayout* layoutOperationParts(); void setupScreen(); virtual void setCurrentPoseSeqItem(PoseSeqItemPtr poseSeqItem); void requestRowRectsUpdate(); void onTimeLengthChanged(double value); void onPoseTimeSpinChanged(double value); void onPoseTTimeSpinChanged(double value); virtual void onInsertPoseButtonClicked(); virtual void onPoseInserted(PoseSeq::iterator it, bool isMoving); virtual void onPoseRemoving(PoseSeq::iterator it, bool isMoving); virtual void onPoseModified(PoseSeq::iterator it); void onCurrentTimeSpinChanged(double value); void setCurrentTime(double time, bool blockScroll = false); virtual bool onTimeChanged(double time); void setTimeOfScreenLeft(double time, bool changeScrollBar = true, bool forceChange = false); void onHScrollbarChanged(double value); void onGridResolutionChanged(double value); virtual void onLinkTreeUpdateRequest(bool isInitialCreation); void updateRowRects(); void updateRowRectsSub(QTreeWidgetItem* treeWidgetItem); LinkTreeItem* getFirstVisibleAncestor(const LinkTreeItem* item); bool checkIfPoseHasRow(const PosePtr& pose, const LinkTreeItem* item); double searchLastPoseTime(const LinkTreeItem* item); void processKeyPoseMarkersSub(LinkTreeItem* item, boost::function func); void processKeyPoseMarkers(boost::function func); bool onScreenPaintEvent(QPaintEvent* event); void drawBackground(); void drawTimeCursor(); void drawKeyPoseMarker(); virtual void onTimeScaleChanged(); virtual void onSelectedPosesModified(); void pickPose(); void pickPoseSub(); void onScreenFontChanged(); bool onScreenResizeEvent(QResizeEvent* event); bool onScreenMouseButtonPressEvent(QMouseEvent* event); void pickPoseOnButtonPress(bool isAdding); bool onScreenMouseButtonReleaseEvent(QMouseEvent* event); bool onScreenMouseMoveEvent(QMouseEvent* event); void pickPoseOnMotionNotify(); void dragSelectedPoses(); void dragTransitionTime(); void dragScaling(); bool onScreenKeyPressEvent(QKeyEvent* event); bool onScreenKeyReleaseEvent(QKeyEvent* event); void selectPrevPose(bool isAdding); void selectNextPose(bool isAdding); void onMenuButtonClicked(); bool storeState(Archive& archive); bool restoreState(const Archive& archive); }; } PoseRollView* PoseRollView::instance() { static PoseRollView* instance = new PoseRollView(); return instance; } PoseRollView::PoseRollView() { setName(N_("Pose Roll")); setDefaultLayoutArea(View::BOTTOM); setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Ignored); impl = new PoseRollViewImpl(this); impl->initialize(); } PoseRollViewImpl::PoseRollViewImpl(PoseRollView* self) : PoseSeqViewBase(self), self(self), commandMenuManager(&commandMenu) { } void PoseRollViewImpl::initialize() { timeLength = 10.0; updateRowRectsNeeded = true; isTmpScrollBlocked = false; QVBoxLayout* vbox = new QVBoxLayout(); vbox->setSpacing(0); vbox->addLayout(layoutOperationParts()); setupScreen(); QHBoxLayout* treeBox = new QHBoxLayout(); linkTreeWidget->installEventFilter(self); linkTreeWidget->setAlternatingRowColors(true); treeWidgetVerticalScrollBar = new ScrollBarEx(Qt::Vertical); treeBox->addWidget(treeWidgetVerticalScrollBar); linkTreeWidget->setVerticalScrollBar(treeWidgetVerticalScrollBar); linkTreeWidget->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); treeWidgetVerticalScrollBar->sigValueChanged().connect( bind(&PoseRollViewImpl::requestRowRectsUpdate, this)); // setup the link tree view model linkTreeWidget->sigItemExpanded().connect( bind(&PoseRollViewImpl::requestRowRectsUpdate, this)); linkTreeWidget->sigItemCollapsed().connect( bind(&PoseRollViewImpl::requestRowRectsUpdate, this)); treeBox->addWidget(linkTreeWidget, 1); hScrollBar = new DoubleScrollBar(Qt::Horizontal); hScrollBar->setSingleStep(0.1); hScrollBar->setRange(-leftMargin, timeLength + (2.0 / timeToScreenX)); hScrollBarChangedConnection = hScrollBar->sigValueChanged().connect( bind(&PoseRollViewImpl::onHScrollbarChanged, this, _1)); QHBoxLayout* hbox = new QHBoxLayout(); hbox->setSpacing(8); hbox->addSpacing(16); hbox->addWidget(¤tItemLabel); hbox->addWidget(new QLabel(":")); hbox->addWidget(&poseNameLabel); gridLayout = new QGridLayout(); gridLayout->setSpacing(0); gridLayout->setContentsMargins(0, 0, 0, 0); gridLayout->addLayout(treeBox, 0, 0); gridLayout->addWidget(screen, 0, 1); gridLayout->addLayout(hbox, 1, 0); gridLayout->addWidget(hScrollBar, 1, 1); gridLayout->setColumnStretch(1, 1); vbox->addLayout(gridLayout, 1); self->setLayout(vbox); commandMenuManager.addItem(_("Select specified key poses"))->sigTriggered().connect( bind(&PoseRollViewImpl::onSelectSpecifiedKeyPosesActivated, this)); commandMenuManager.addItem(_("Adjust step positions"))->sigTriggered().connect( bind(&PoseRollViewImpl::onAdjustStepPositionsActivated, this)); commandMenuManager.addItem(_("Adjust waist positions of selected key poses"))->sigTriggered().connect( bind(&PoseRollViewImpl::onAdjustWaistPositionActivated, this)); commandMenuManager.addItem(_("Rotate yaw orientations"))->sigTriggered().connect( bind(&PoseRollViewImpl::onRotateYawOrientationsActivated, this)); commandMenuManager.addItem(_("Update key poses with balanced trajectories"))->sigTriggered().connect( bind(&PoseRollViewImpl::onUpdateKeyposesWithBalancedTrajectoriesActivated, this)); commandMenuManager.addItem(_("Flip poses against the x-z plane"))->sigTriggered().connect( bind(&PoseRollViewImpl::onFlipPosesActivated, this)); lipSyncCheck = commandMenuManager.addCheckItem(_("Show lip-sync elements")); lipSyncCheck->sigToggled().connect(bind(&QWidget::update, screen)); commandMenuButton.setText(_("Menu")); commandMenuButton.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); commandMenuButton.sigClicked().connect(bind(&PoseRollViewImpl::onMenuButtonClicked, this)); } PoseRollView::~PoseRollView() { delete impl; } PoseRollViewImpl::~PoseRollViewImpl() { } QHBoxLayout* PoseRollViewImpl::layoutOperationParts() { QHBoxLayout* hbox = new QHBoxLayout(); hbox->setSpacing(0); hbox->addWidget(&commandMenuButton); updateButton.setEnabled(false); deleteButton.setEnabled(false); timeSyncCheck.setText(_("Sync")); hbox->addWidget(new QLabel(_("T:"))); currentTimeSpin.setToolTip(_("Current time")); currentTimeSpin.setAlignment(Qt::AlignCenter); currentTimeSpin.setDecimals(3); currentTimeSpin.setRange(0.0, 999.999); currentTimeSpin.setSingleStep(0.005); currentTimeSpinConnection = currentTimeSpin.sigValueChanged().connect( bind(&PoseRollViewImpl::onCurrentTimeSpinChanged, this, _1)); hbox->addWidget(¤tTimeSpin); hbox->addWidget(new QLabel(" / ")); timeLengthSpin.setToolTip(_("Time length for editing")); timeLengthSpin.setAlignment(Qt::AlignCenter); timeLengthSpin.setDecimals(0); timeLengthSpin.setRange(1.0, 999.0); timeLengthSpin.setSingleStep(1.0); timeLengthSpin.setValue(timeLength); timeLengthSpin.sigValueChanged().connect( bind(&PoseRollViewImpl::onTimeLengthChanged, this, _1)); hbox->addWidget(&timeLengthSpin); hbox->addWidget(&timeSyncCheck); hbox->addWidget(&insertPoseButton); hbox->addWidget(new QLabel(_("TT:"))); hbox->addWidget(&transitionTimeSpin); hbox->addWidget(&updateButton); hbox->addWidget(&updateAllToggle); hbox->addWidget(&autoUpdateModeCheck); hbox->addWidget(new QLabel(_("T:"))); poseTimeSpin.setToolTip(_("Time of the selected pose")); poseTimeSpin.setAlignment(Qt::AlignCenter); poseTimeSpin.setEnabled(false); poseTimeSpin.setDecimals(3); poseTimeSpin.setRange(0.0, 999.999); poseTimeSpin.setSingleStep(0.005); poseTimeSpinConnection = poseTimeSpin.sigValueChanged().connect( bind(&PoseRollViewImpl::onPoseTimeSpinChanged, this, _1)); hbox->addWidget(&poseTimeSpin); hbox->addWidget(new QLabel(_("TT:"))); poseTTimeSpin.setToolTip(_("Transition time of the selected pose")); poseTTimeSpin.setAlignment(Qt::AlignCenter); poseTTimeSpin.setEnabled(false); poseTTimeSpin.setDecimals(3); poseTTimeSpin.setRange(0.0, 9.999); poseTTimeSpin.setSingleStep(0.005); poseTTimeSpinConnection = poseTTimeSpin.sigValueChanged().connect( bind(&PoseRollViewImpl::onPoseTTimeSpinChanged, this, _1)); hbox->addWidget(&poseTTimeSpin); hbox->addWidget(&deleteButton); hbox->addWidget(new QLabel(_("Grid:"))); gridIntervalSpin.setAlignment(Qt::AlignCenter); gridIntervalSpin.setDecimals(0); gridIntervalSpin.setRange(1.0, 10.0); gridIntervalSpin.setSingleStep(1.0); gridIntervalSpin.sigValueChanged().connect( bind(&PoseRollViewImpl::onGridResolutionChanged, this, _1)); hbox->addWidget(&gridIntervalSpin); hbox->addStretch(); return hbox; } void PoseRollViewImpl::setupScreen() { screen = new QWidget(); screen->setMouseTracking(true); screen->installEventFilter(self); screen->setBackgroundRole(QPalette::Base); screen->setAutoFillBackground(true); left = -leftMargin; right = 0.0; screenWidth = 0.0; screenHeight = 0.0; timeToScreenX = 120.0; gridStep = 1.0; dash.push_back(2.0); dash.push_back(2.0); markerPen.setWidth(2); markerPen.setColor(Qt::black); pronumSymbolPen.setWidth(2); pronumSymbolPen.setColor(Qt::darkGreen); selectedMarkerPen.setWidth(3); selectedMarkerPen.setColor(Qt::red); gridPen.setWidth(1); gridPen.setDashPattern(dash); gridPen.setColor(QColor(50, 50, 50)); cursorPen.setWidth(1); cursorPen.setColor(Qt::white); } bool PoseRollView::eventFilter(QObject* obj, QEvent* event) { if(obj == impl->linkTreeWidget){ switch(event->type()){ case QEvent::FontChange: case QEvent::StyleChange: case QEvent::LanguageChange: case QEvent::LocaleChange: impl->requestRowRectsUpdate(); return false; break; } } else if(obj == impl->screen){ switch(event->type()){ case QEvent::Paint: return impl->onScreenPaintEvent(static_cast(event)); case QEvent::Resize: return impl->onScreenResizeEvent(static_cast(event)); case QEvent::MouseMove: return impl->onScreenMouseMoveEvent(static_cast(event)); case QEvent::MouseButtonPress: return impl->onScreenMouseButtonPressEvent(static_cast(event)); case QEvent::MouseButtonRelease: return impl->onScreenMouseButtonReleaseEvent(static_cast(event)); case QEvent::KeyPress: return impl->onScreenKeyPressEvent(static_cast(event)); case QEvent::KeyRelease: return impl->onScreenKeyReleaseEvent(static_cast(event)); default: return false; } } return View::eventFilter(obj, event); } void PoseRollViewImpl::setCurrentPoseSeqItem(PoseSeqItemPtr poseSeqItem) { BodyPtr prevBody = body; PoseSeqViewBase::setCurrentPoseSeqItem(poseSeqItem); if(poseSeqItem){ double lower, upper; poseSeqItem->poseSeq()->getDomain(lower, upper); if(timeLengthSpin.value() < upper){ timeLengthSpin.setValue(upper); } } if(body != prevBody){ updateRowRectsNeeded = true; } screen->update(); } void PoseRollViewImpl::requestRowRectsUpdate() { updateRowRectsNeeded = true; screen->update(); } void PoseRollViewImpl::onTimeLengthChanged(double value) { timeLength = value; hScrollBar->setRange(-leftMargin, timeLength + (2.0 / timeToScreenX)); if(currentTime > timeLength){ setCurrentTime(timeLength); } else { screen->update(); } } void PoseRollViewImpl::onPoseTimeSpinChanged(double value) { if(!selectedPoseIters.empty()){ double scaledTime = value; double time = scaledTime / timeScale; if(time != (*selectedPoseIters.begin())->time()){ currentPoseSeqItem->beginEditing(); if(currentPoseSeqItem->endEditing(moveSelectedPoses(time))){ doAutomaticInterpolationUpdate(); } setCurrentTime(scaledTime); } } } void PoseRollViewImpl::onPoseTTimeSpinChanged(double value) { if(!selectedPoseIters.empty()){ double scaledTTime = value; double ttime = scaledTTime / timeScale; currentPoseSeqItem->beginEditing(); if(currentPoseSeqItem->endEditing( modifyTransitionTimeOfSelectedPoses(ttime))){ doAutomaticInterpolationUpdate(); } } } void PoseRollView::onInsertPoseButtonClicked() { return impl->onInsertPoseButtonClicked(); } void PoseRollViewImpl::onInsertPoseButtonClicked() { if(currentPoseSeqItem){ currentPoseSeqItem->beginEditing(); PoseSeq::iterator poseIter = insertPose(); currentPoseSeqItem->endEditing(poseIter != seq->end()); } } void PoseRollViewImpl::onPoseInserted(PoseSeq::iterator it, bool isMoving) { if(TRACE_FUNCTIONS){ cout << "PoseRollViewImpl::onPoseInserted()" << endl; } PoseSeqViewBase::onPoseInserted(it, isMoving); screen->update(); } void PoseRollViewImpl::onPoseRemoving(PoseSeq::iterator it, bool isMoving) { if(TRACE_FUNCTIONS){ cout << "PoseRollViewImpl::onPoseRemoving()" << endl; } PoseSeqViewBase::onPoseRemoving(it, isMoving); if(!isMoving){ screen->update(); } } void PoseRollViewImpl::onPoseModified(PoseSeq::iterator it) { PoseSeqViewBase::onPoseModified(it); screen->update(); } void PoseRollViewImpl::onCurrentTimeSpinChanged(double value) { if(value > timeLength){ timeLengthSpin.setValue(value); } setCurrentTime(value); } void PoseRollViewImpl::setCurrentTime(double time, bool blockScroll) { time = std::max(0.0, time); isTmpScrollBlocked = blockScroll; onTimeChanged(time); isTmpScrollBlocked = false; if(timeSyncCheck.isChecked()){ connectionOfTimeChanged.block(); timeBar->setTime(time); connectionOfTimeChanged.unblock(); } } bool PoseRollViewImpl::onTimeChanged(double time) { if(TRACE_FUNCTIONS){ cout << "PoseRollViewImpl::onTimeChanged(" << time << ")" << endl; } const double width = screenWidth / timeToScreenX; bool newTimeInCurrentVisibleRange = (time >= left && time < right); // Scroll sync if(!newTimeInCurrentVisibleRange && !isTmpScrollBlocked){ if(time < left){ if(left - time < width / 3.0){ setTimeOfScreenLeft(left - width * 0.9); } else { setTimeOfScreenLeft(time - width / 2.0); } } else { if(time - right < width / 3.0){ setTimeOfScreenLeft(left + width * 0.9); } else { setTimeOfScreenLeft(time - width / 2.0); } } } else if(newTimeInCurrentVisibleRange || (currentTime >= left && currentTime < right)){ screen->update(); } if(time != currentTime){ currentTime = time; currentTimeSpinConnection.block(); currentTimeSpin.setValue(time); currentTimeSpinConnection.unblock(); } return (seq && (time < timeScale * seq->endingTime())); } void PoseRollViewImpl::setTimeOfScreenLeft(double time, bool changeScrollBar, bool forceChange) { time = std::max(-leftMargin, std::min(timeLength, time)); if(time != left || forceChange){ left = time; right = left + screenWidth / timeToScreenX; if(changeScrollBar){ hScrollBarChangedConnection.block(); hScrollBar->setValue(left); hScrollBarChangedConnection.unblock(); } screen->update(); } } void PoseRollViewImpl::onHScrollbarChanged(double value) { setTimeOfScreenLeft(value, false); } void PoseRollViewImpl::onGridResolutionChanged(double value) { gridStep = 1.0 / value; screen->update(); } void PoseRollViewImpl::onLinkTreeUpdateRequest(bool isInitialCreation) { PoseSeqViewBase::onLinkTreeUpdateRequest(isInitialCreation); itemIndexToRowInfoMap.resize(linkTreeWidget->numLinkTreeItems()); updateRowRectsNeeded = true; } void PoseRollViewImpl::updateRowRects() { if(TRACE_FUNCTIONS){ cout << "PoseRollViewImpl::updateRowRects(): updateRowRectsNeeded = " << updateRowRectsNeeded << endl; } if(updateRowRectsNeeded){ linkIndexToVisibleRowAncestorMap.clear(); visibleRows.clear(); if(body){ linkIndexToVisibleRowAncestorMap.resize(body->numLinks()); updateRowRectsSub(linkTreeWidget->invisibleRootItem()); rowRenderInfos.resize(visibleRows.size()); } treeTopOnScreen = linkTreeWidget->header()->geometry().bottom(); treeBottomOnScreen = treeTopOnScreen; if(!visibleRows.empty()){ RowInfo& bottomRow = itemIndexToRowInfoMap[visibleRows.back()->rowIndex()]; treeBottomOnScreen += bottomRow.y + bottomRow.height; } updateRowRectsNeeded = false; } } void PoseRollViewImpl::updateRowRectsSub(QTreeWidgetItem* treeWidgetItem) { LinkTreeItem* item = dynamic_cast(treeWidgetItem); if(item){ QRect rect = linkTreeWidget->visualItemRect(item); RowInfo& rowInfo = itemIndexToRowInfoMap[item->rowIndex()]; if(rect.isEmpty()){ rowInfo.isVisible = false; rowInfo.visibleRowIndex = -1; rowInfo.y = 0; rowInfo.height = 0; } else { rowInfo.isVisible = true; rowInfo.visibleRowIndex = visibleRows.size(); rowInfo.y = rect.y(); rowInfo.height = rect.height(); visibleRows.push_back(item); } Link* link = item->link(); if(!link){ rowInfo.linkIndex = -1; rowInfo.jointId = -1; if(item == zmpRow){ visibleRowAncestorOfZmp = rowInfo.isVisible ? item : getFirstVisibleAncestor(item); } } else { rowInfo.linkIndex = link->index; rowInfo.jointId = link->jointId; linkIndexToVisibleRowAncestorMap[link->index] = rowInfo.isVisible ? item : getFirstVisibleAncestor(item); } } int n = treeWidgetItem->childCount(); for(int i=0; i < n; ++i){ updateRowRectsSub(treeWidgetItem->child(i)); } } LinkTreeItem* PoseRollViewImpl::getFirstVisibleAncestor(const LinkTreeItem* item) { for(QTreeWidgetItem* parent = item->parent(); parent; parent = parent->parent()){ LinkTreeItem* row = dynamic_cast(parent); if(row){ const RowInfo& info = itemIndexToRowInfoMap[row->rowIndex()]; if(info.isVisible){ return row; } } } return 0; } bool PoseRollViewImpl::checkIfPoseHasRow(const PosePtr& pose, const LinkTreeItem* item) { if(item == zmpRow && pose->isZmpValid()){ return true; } const RowInfo& info = itemIndexToRowInfoMap[item->rowIndex()]; if(info.jointId >= 0 && pose->isJointValid(info.jointId)){ return true; } int n = item->childCount(); for(int i=0; i < n; ++i){ LinkTreeItem* childItem = dynamic_cast(item->child(i)); if(childItem && checkIfPoseHasRow(pose, childItem)){ return true; } } return false; } double PoseRollViewImpl::searchLastPoseTime(const LinkTreeItem* item) { PoseSeq::iterator last = markerPoseIter; while(last != seq->begin()){ last--; PosePtr pose = last->get(); if(pose){ if(checkIfPoseHasRow(pose, item)){ break; } } } return timeScale * last->time(); } void PoseRollViewImpl::processKeyPoseMarkersSub(LinkTreeItem* item, boost::function func) { while(item){ const RowInfo& rowInfo = itemIndexToRowInfoMap[item->rowIndex()]; RowRenderInfo& renderInfo = rowRenderInfos[rowInfo.visibleRowIndex]; if(renderInfo.rendered){ break; } else { renderInfo.rendered = true; if(renderInfo.lastPoseTime == -std::numeric_limits::max()){ renderInfo.lastPoseTime = searchLastPoseTime(item); } double t0 = std::max(markerTime0, renderInfo.lastPoseTime); markerX0 = floor(timeToScreenX * (t0 - left)); markerY0 = rowInfo.y + treeTopOnScreen; markerY1 = markerY0 + rowInfo.height; func(); if(!isMarkerPronunSymbol){ renderInfo.lastPoseTime = timeScale * markerPoseIter->time(); } } item = dynamic_cast(item->parent()); } } void PoseRollViewImpl::processKeyPoseMarkers(boost::function func) { for(size_t i=0; i < rowRenderInfos.size(); ++i){ RowRenderInfo& info = rowRenderInfos[i]; if(FAST_DRAW_MODE){ info.lastPoseTime = left; } else { info.lastPoseTime = -std::numeric_limits::max(); } } markerPoseIter = seq->seek(seq->begin(), left / timeScale); const std::vector& lipSyncLinkIndices = currentPoseSeqItem->interpolator()->lipSyncLinkIndices(); while(markerPoseIter != seq->end()){ if(FAST_DRAW_MODE){ if(timeScale * markerPoseIter->time() > right){ break; } } double time = timeScale * markerPoseIter->time(); double ttime = timeScale * markerPoseIter->maxTransitionTime(); if(ttime == 0.0){ markerTime0 = -std::numeric_limits::max(); } else { markerTime0 = time - ttime; } for(size_t i=0; i < rowRenderInfos.size(); ++i){ RowRenderInfo& renderInfo = rowRenderInfos[i]; renderInfo.rendered = false; } isMarkerSelected = (findPoseIterInSelected(markerPoseIter) != selectedPoseIters.end()); markerX1 = floor(timeToScreenX * (time - left)); PosePtr pose = markerPoseIter->get(); if(pose){ isMarkerPronunSymbol = false; int n = std::min(body->numJoints(), pose->numJoints()); for(int i=0; i < n; ++i){ Link* joint = body->joint(i); if(pose->isJointValid(i) && joint->isValid()){ processKeyPoseMarkersSub(linkIndexToVisibleRowAncestorMap[joint->index], func); } } for(Pose::LinkInfoMap::iterator it = pose->ikLinkBegin(); it != pose->ikLinkEnd(); ++it){ processKeyPoseMarkersSub(linkIndexToVisibleRowAncestorMap[it->first], func); } if(pose->isZmpValid()){ processKeyPoseMarkersSub(visibleRowAncestorOfZmp, func); } } else if(lipSyncCheck->isChecked()) { PronunSymbolPtr pronun = markerPoseIter->get(); if(pronun){ isMarkerPronunSymbol = true; for(size_t i=0; i < lipSyncLinkIndices.size(); ++i){ processKeyPoseMarkersSub(linkIndexToVisibleRowAncestorMap[lipSyncLinkIndices[i]], func); } } } ++markerPoseIter; } } bool PoseRollViewImpl::onScreenPaintEvent(QPaintEvent* event) { if(TRACE_FUNCTIONS){ cout << "PoseRollViewImpl::onScreenPaintEvent()" << endl; } updateRowRects(); painter.begin(screen); drawBackground(); painter.setClipRect(0.0, treeTopOnScreen, screenWidth, screenHeight - treeTopOnScreen); painter.setClipping(true); if(seq){ processKeyPoseMarkers(bind(&PoseRollViewImpl::drawKeyPoseMarker, this)); } painter.setClipping(false); drawTimeCursor(); painter.end(); return false; } void PoseRollViewImpl::drawBackground() { double bodyHeight = screenHeight - treeTopOnScreen; double rowBottom = 0.0; double offset = treeTopOnScreen; painter.setPen(gridPen); // draw row lines if(!visibleRows.empty()){ double x = timeToScreenX * (floor(left) - left); for(size_t i=0; i < visibleRows.size(); ++i){ LinkTreeItem* item = visibleRows[i]; const RowInfo& r = itemIndexToRowInfoMap[item->rowIndex()]; if(r.y < 0.0){ continue; } if(r.y > bodyHeight){ break; } double y = r.y + offset; painter.drawLine(x, y, screenWidth, y); } const RowInfo& r = itemIndexToRowInfoMap[visibleRows.back()->rowIndex()]; double y = r.y + r.height + offset; rowBottom = std::min(y, (double)screenHeight); painter.drawLine(x, y, screenWidth, y); } // draw time grid lines double t = floor(left + 0.9999); // for avoiding -0.0 while(true){ double x = timeToScreenX * (t - left); if(x > screenWidth){ break; } x = floor(x) /* + 0.5 */; QString timeText = QString::number(t, 'f', 1); QFontMetrics metrics(painter.fontMetrics()); QRect r = metrics.boundingRect(timeText); double tx = x - r.width() / 2.0; double ty = treeTopOnScreen / 2.0 - (r.height() / 2.0 - metrics.ascent()); painter.drawText(tx, ty, timeText); painter.drawLine(x, treeTopOnScreen, x, rowBottom); t += gridStep; } } void PoseRollViewImpl::drawKeyPoseMarker() { QPen* pen; if(isMarkerSelected){ if(FAST_DRAW_MODE){ painter.setBrush(QBrush(QColor(255, 200, 200))); } else { QLinearGradient gradient(markerX0, 0.0, markerX1, 0.0); gradient.setColorAt(0.0, QColor(255, 240, 240)); gradient.setColorAt(1.0, QColor(255, 150, 150)); painter.setBrush(gradient); } pen = &selectedMarkerPen; } else { if(FAST_DRAW_MODE){ painter.setBrush(QBrush(QColor(180, 180, 180))); } else { QLinearGradient gradient(markerX0, 0.0, markerX1, 0.0); gradient.setColorAt(0.0, QColor(245, 245, 245)); gradient.setColorAt(1.0, QColor(15, 15, 15)); painter.setBrush(gradient); } pen = isMarkerSelected ? &pronumSymbolPen : &markerPen; } QPointF points[3]; points[0] = QPointF(markerX0, markerY1); points[1] = QPointF(markerX1, markerY0); points[2] = QPointF(markerX1, markerY1); painter.setPen(Qt::NoPen); painter.drawConvexPolygon(points, 3); painter.setPen(*pen); painter.setBrush(Qt::NoBrush); painter.drawLine(points[1], points[2]); } void PoseRollViewImpl::drawTimeCursor() { double x = floor(timeToScreenX * (currentTime - left)) /* + 0.5 */; if(x >= 0.0 && x < screenWidth){ painter.setPen(cursorPen); painter.setCompositionMode(QPainter::RasterOp_SourceXorDestination); painter.drawLine(x, 0, x, screenHeight); painter.setCompositionMode(QPainter::CompositionMode_SourceOver); }; } void PoseRollViewImpl::onTimeScaleChanged() { PoseSeqViewBase::onTimeScaleChanged(); screen->update(); } void PoseRollViewImpl::onSelectedPosesModified() { PoseSeqViewBase::onSelectedPosesModified(); poseTimeSpinConnection.block(); poseTTimeSpinConnection.block(); if(selectedPoseIters.empty()){ poseNameLabel.setText(""); poseTimeSpin.setEnabled(false); poseTimeSpin.setValue(0.0); poseTTimeSpin.setEnabled(false); poseTTimeSpin.setValue(0.0); } else { PoseSeq::iterator poseIter = *selectedPoseIters.begin(); poseNameLabel.setText(poseIter->name().c_str()); poseTimeSpin.setEnabled(true); poseTimeSpin.setValue(timeScale * poseIter->time()); poseTTimeSpin.setEnabled(true); poseTTimeSpin.setValue(timeScale * poseIter->maxTransitionTime()); } poseTTimeSpinConnection.unblock(); poseTimeSpinConnection.unblock(); screen->update(); } void PoseRollViewImpl::pickPose() { if(seq){ pickedPoseIter = seq->end(); pickDistance = std::numeric_limits::max(); pickedPart = PICK_NONE; processKeyPoseMarkers(bind(&PoseRollViewImpl::pickPoseSub, this)); } } void PoseRollViewImpl::pickPoseSub() { if(markerY0 <= pointerY && pointerY < markerY1){ if(!isMarkerPronunSymbol){ const double margin = 2.0; if((markerX0 - margin) <= pointerX && pointerX <= (markerX1 + margin)){ double dLeft = pointerX - markerX0; if(dLeft < 0.0 || dLeft >= 6.0){ dLeft = std::numeric_limits::max(); } double dRight = fabs(markerX1 - pointerX); double d; PickedPart part; if(dLeft < dRight){ d = dLeft; part = PICK_LEFT; pickedTime = (markerX0 / timeToScreenX) + left; } else { d = dRight; pickedTime = (markerX1 / timeToScreenX) + left; if(d <= margin) { part = PICK_RIGHT; } else { part = PICK_BODY; } } if(d < pickDistance){ pickDistance = d; pickedPoseIter = markerPoseIter; pickedPart = part; } } } } } bool PoseRollViewImpl::onScreenResizeEvent(QResizeEvent* event) { screenWidth = event->size().width(); screenHeight = event->size().height(); hScrollBar->setPageStep(screenWidth / timeToScreenX); right = left + screenWidth / timeToScreenX; screen->update(); return false; } bool PoseRollViewImpl::onScreenMouseButtonPressEvent(QMouseEvent* event) { screen->setFocus(Qt::MouseFocusReason); pressedScreenX = event->x(); pointerX = event->x(); pointerY = event->y(); dragOrgLeft = left; dragMode = DRAG_NONE; dragState = DS_INITIAL; if(event->type() == QEvent::MouseButtonPress){ if(event->button() == Qt::LeftButton){ if(pointerY < treeTopOnScreen || pointerY > treeBottomOnScreen){ setCurrentTime(pointerX / timeToScreenX + left); dragMode = DRAG_TIME_CURSOR; } else { pickPoseOnButtonPress(event->modifiers() & Qt::ControlModifier); } } else if(event->button() == Qt::MidButton){ dragMode = DRAG_SCALING; dragOrgScale = timeToScreenX; } else if(event->button() == Qt::RightButton){ popupContextMenu(event); } } return true; } void PoseRollViewImpl::pickPoseOnButtonPress(bool isAdding) { if(!seq){ return; } pickPose(); toggleSelection(pickedPoseIter, isAdding, true); if(pickedPoseIter != seq->end()){ if(pickedPart == PICK_RIGHT){ dragMode = DRAG_POSES; draggingPosesOrgTime = timeScale * (*selectedPoseIters.begin())->time(); screen->setCursor(Qt::ClosedHandCursor); } else if(pickedPart == PICK_LEFT){ dragMode = DRAG_TRANSITION_TIME; screen->setCursor(Qt::SplitHCursor); } } } bool PoseRollViewImpl::onScreenMouseMoveEvent(QMouseEvent* event) { if(TRACE_FUNCTIONS){ cout << "PoseRollViewImpl::onScreenMotionNotifyEvent()" << endl; } pointerX = event->x(); pointerY = event->y(); switch(dragMode){ case DRAG_NONE: screen->setCursor(Qt::ArrowCursor); pickPoseOnMotionNotify(); break; case DRAG_POSES: dragSelectedPoses(); break; case DRAG_TRANSITION_TIME: dragTransitionTime(); break; case DRAG_TIME_CURSOR: setCurrentTime(pointerX / timeToScreenX + left, true); break; case DRAG_SCALING: dragScaling(); break; default: break; } return true; } bool PoseRollViewImpl::onScreenMouseButtonReleaseEvent(QMouseEvent* event) { switch(dragMode){ case DRAG_POSES: case DRAG_TRANSITION_TIME: if(dragState == DS_DRAGGED){ currentPoseSeqItem->endEditing(); doAutomaticInterpolationUpdate(); } break; default: break; } dragMode = DRAG_NONE; screen->setCursor(Qt::ArrowCursor); return true; } void PoseRollViewImpl::pickPoseOnMotionNotify() { if(seq && !updateRowRectsNeeded){ pickPose(); if(pickedPoseIter != seq->end()){ if(pickedPart == PICK_LEFT){ screen->setCursor(Qt::SplitHCursor); } else if(pickedPart == PICK_RIGHT){ screen->setCursor(Qt::OpenHandCursor); } } } } void PoseRollViewImpl::dragSelectedPoses() { if(dragState == DS_INITIAL){ currentPoseSeqItem->beginEditing(); dragState = DS_DRAGGED; } double scaledTime = draggingPosesOrgTime + (pointerX - pressedScreenX) / timeToScreenX; moveSelectedPoses(scaledTime / timeScale); } void PoseRollViewImpl::dragTransitionTime() { if(dragState == DS_INITIAL){ currentPoseSeqItem->beginEditing(); dragState = DS_DRAGGED; } seq->beginPoseModification(pickedPoseIter); double scaledTime = pickedTime + (pointerX - pressedScreenX) / timeToScreenX; double ttime = pickedPoseIter->time() - scaledTime / timeScale; pickedPoseIter->setMaxTransitionTime(std::max(ttime, 0.0)); seq->endPoseModification(pickedPoseIter); } void PoseRollViewImpl::dragScaling() { double zoomRatio = pow(1.01, pointerX - pressedScreenX); timeToScreenX = dragOrgScale * zoomRatio; double dpx = pressedScreenX / dragOrgScale; double dx = dpx * (zoomRatio - 1.0) / zoomRatio; hScrollBarChangedConnection.block(); hScrollBar->setPageStep(screenWidth / timeToScreenX); hScrollBarChangedConnection.unblock(); setTimeOfScreenLeft(dragOrgLeft + dx, true, true); } bool PoseRollViewImpl::onScreenKeyPressEvent(QKeyEvent* event) { bool handled = false; bool isCtrlActive = (event->modifiers() & Qt::ControlModifier); if(isCtrlActive){ handled = true; switch(event->key()){ case Qt::Key_A: selectAllPoses(); break; case Qt::Key_X: cutSelectedPoses(); break; case Qt::Key_C: copySelectedPoses(); break; case Qt::Key_V: pasteCopiedPoses(currentTime / timeScale); break; case Qt::Key_Z: if(currentPoseSeqItem){ if(event->modifiers() & Qt::ShiftModifier){ currentPoseSeqItem->redo(); } else { currentPoseSeqItem->undo(); } } break; default: handled = false; break; } } if(!handled){ handled = true; switch(event->key()){ case Qt::Key_Left: selectPrevPose(isCtrlActive); break; case Qt::Key_Right: selectNextPose(isCtrlActive); break; default: handled = false; break; } } return handled; } bool PoseRollViewImpl::onScreenKeyReleaseEvent(QKeyEvent* event) { return false; } void PoseRollViewImpl::selectPrevPose(bool isAdding) { if(!selectedPoseIters.empty()){ PoseSeq::iterator it = *selectedPoseIters.begin(); if(it != seq->begin()){ it--; } while(true){ if(!lipSyncCheck->isChecked() && !it->get()){ if(it != seq->begin()){ it--; continue; } else { it = seq->end(); } } break; } if(it != seq->end()){ toggleSelection(it, isAdding, true); } } } void PoseRollViewImpl::selectNextPose(bool isAdding) { if(!selectedPoseIters.empty()){ PoseSeq::iterator it = *(--selectedPoseIters.end()); ++it; if(!lipSyncCheck->isChecked()){ while(it != seq->end() && !it->get()){ ++it; } } if(it != seq->end()){ toggleSelection(it, isAdding, true); } } } void PoseRollViewImpl::onMenuButtonClicked() { commandMenu.exec(commandMenuButton.mapToGlobal(QPoint(0,0))); } bool PoseRollView::storeState(Archive& archive) { return impl->storeState(archive); } bool PoseRollViewImpl::storeState(Archive& archive) { if(PoseSeqViewBase::storeState(archive)){ if(!timeSyncCheck.isChecked()){ archive.write("time", currentTime); } archive.write("timeLength", timeLength); archive.write("showLipSync", lipSyncCheck->isChecked()); archive.write("gridInterval", gridIntervalSpin.value()); return true; } return false; } bool PoseRollView::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool PoseRollViewImpl::restoreState(const Archive& archive) { updateRowRectsNeeded = true; timeLengthSpin.setValue(archive.get("timeLength", timeLengthSpin.value())); lipSyncCheck->setChecked(archive.get("showLipSync", lipSyncCheck->isChecked())); gridIntervalSpin.setValue(archive.get("gridInterval", gridIntervalSpin.value())); PoseSeqViewBase::restoreState(archive); if(!timeSyncCheck.isChecked()){ double time; if(archive.read("time", time)){ currentTimeSpin.setValue(time); } } return true; } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseRollView.h000066400000000000000000000015461207742442300226640ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_CHOREOGRAPHY_POSE_ROLL_VIEW_H_INCLUDED #define CNOID_CHOREOGRAPHY_POSE_ROLL_VIEW_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { class Archive; class PoseRollViewImpl; class CNOID_EXPORT PoseRollView : public cnoid::View { public: static PoseRollView* instance(); PoseRollView(); ~PoseRollView(); /** This is a function for a temporary test. Please don't use this function. */ void onInsertPoseButtonClicked(void); private: PoseRollViewImpl* impl; virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); virtual bool eventFilter(QObject *obj, QEvent *event); }; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeq.cpp000066400000000000000000000317611207742442300222060ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "PoseSeq.h" #include "PronunSymbol.h" #include "LipSyncTranslator.h" #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; PoseRef::PoseRef(PoseSeq* owner, PoseUnitPtr poseUnit, double time) : poseUnit_(poseUnit) { this->owner = owner; time_ = time; maxTransitionTime_ = 0.0; } PoseSeq::PoseSeq() { } PoseSeq::PoseSeq(const PoseSeq& org) : PoseUnit(org) { iterator current = begin(); for(const_iterator it = org.begin(); it != org.end(); ++it){ current = copyElement(current, it); } } PoseSeq::~PoseSeq() { } void PoseSeq::setName(const std::string& name) { name_ = name; } PoseSeq::iterator PoseSeq::copyElement(iterator seekpos, const_iterator org, double offset) { bool inserted = false; iterator pos = seekpos; const string& name = org->name(); if(!name.empty()){ PoseUnitMap::iterator p = poseUnitMap.find(name); if(p != poseUnitMap.end()){ pos = insert(seekpos, org->time() + offset, org->name()); pos->setMaxTransitionTime(org->maxTransitionTime()); inserted = true; } } if(!inserted){ PoseUnitPtr orgPoseUnit = org->poseUnit(); if(orgPoseUnit){ PoseUnitPtr copiedUnit(orgPoseUnit->duplicate()); pos = insert(seekpos, org->time() + offset, copiedUnit); pos->setMaxTransitionTime(org->maxTransitionTime()); } } return seekpos; } PoseUnit* PoseSeq::duplicate() { return new PoseSeq(*this); } bool PoseSeq::load(const std::string& filename, const BodyPtr body) { errorMessage_.clear(); /// \todo emit signals refs.clear(); poseUnitMap.clear(); YamlReader parser; if(parser.load(filename)){ const YamlMapping& archive = *parser.document()->toMapping(); restore(archive, body); setName(archive["name"]); return true; } return false; } bool PoseSeq::restore(const YamlMapping& archive, const BodyPtr body) { setTargetBodyName(archive.get("targetBody", body->name())); const YamlSequence& refs = *archive.findSequence("refs"); if(!refs.isValid()){ return false; } PoseSeq::iterator current = begin(); for(int i=0; i < refs.size(); ++i){ const YamlMapping& ref = *refs[i].toMapping(); bool isInserted = false; double time = ref["time"].toDouble(); const YamlNode& referred = ref["refer"]; if(referred.type() == YAML_SCALAR){ const string& name = referred; if(!name.empty()){ current = insert(current, time, name); isInserted = true; } } else if(referred.type() == YAML_MAPPING){ const YamlMapping& mReferred = *referred.toMapping(); const string& type = mReferred["type"]; PoseUnitPtr poseUnit; if(type == "Pose"){ poseUnit = new Pose(); } else if(type == "PronunSymbol"){ poseUnit = new PronunSymbol(); } /* else if(type == "PoseSeq"){ poseUnit = createLocalPoseSeq(); } */ if(poseUnit && poseUnit->restore(mReferred, body)){ poseUnit->name_ = mReferred["name"]; current = insert(current, time, poseUnit); isInserted = true; } } if(isInserted){ current->setMaxTransitionTime(ref.get("maxTransitionTime", 0.0)); } } return true; } bool PoseSeq::save(const std::string& filename, const BodyPtr body) { YamlWriter writer(filename); writer.setKeyOrderPreservationMode(true); storedNames.clear(); YamlMappingPtr archive = new YamlMapping(); archive->setDoubleFormat("%.9g"); store(*archive, body); writer.putComment("Body pose sequence format version 1.0 defined by cnoid-Robotics\n"); writer.putNode(archive); return true; } void PoseSeq::store(YamlMapping& archive, const BodyPtr body) const { archive.write("type", "PoseSeq"); archive.write("name", name(), YAML_DOUBLE_QUOTED); archive.write("targetBody", body->name(), YAML_DOUBLE_QUOTED); YamlSequence& refsNode = *archive.createSequence("refs"); for(PoseRefList::const_iterator p = refs.begin(); p != refs.end(); ++p){ const PoseRef& ref = *p; YamlMappingPtr refNode = refsNode.newMapping(); refNode->write("time", ref.time()); if(ref.maxTransitionTime() > 0.0){ refNode->write("maxTransitionTime", ref.maxTransitionTime()); } const string& name = ref.name(); if((storedNames.find(name) == storedNames.end() /* && !ref.isExternalReference()*/) || name.empty()){ const_cast(this)->storedNames.insert(name); YamlMappingPtr childNode = refNode->createMapping("refer"); ref.poseUnit()->store(*childNode, body); } else { refNode->write("refer", name, YAML_DOUBLE_QUOTED); } } } bool PoseSeq::exportTalkPluginFile(const std::string& filename) { ofstream ofs(filename.c_str()); double standardTransitionTiem = 0.135; double prevTime = 0.0; string prevSymbol; static const bool ENABLE_QUICK_MODE = true; if(ENABLE_QUICK_MODE){ if(!refs.empty()){ bool isInitial = true; for(PoseRefList::iterator p = refs.begin(); p != refs.end(); ++p){ PoseRef& ref = *p; PronunSymbolPtr symbol = ref.get(); if(symbol && !symbol->name().empty()){ const double time = ref.time(); if(isInitial){ isInitial = false; } else { double durationTime = time - prevTime; if(durationTime <= 0.6){ ofs << prevSymbol << " " << durationTime << "\n"; } else { ofs << prevSymbol << " " << 0.6 << "\n"; ofs << "n" << " " << (durationTime - 0.6) << "\n"; } } prevTime = time; prevSymbol = symbol->name(); } } ofs << prevSymbol << " " << standardTransitionTiem << "\n"; } } else { for(PoseRefList::iterator p = refs.begin(); p != refs.end(); ++p){ PoseRef& ref = *p; PronunSymbolPtr symbol = ref.get(); if(symbol && !symbol->name().empty()){ const double time = ref.time(); double transitionTime = time - prevTime; ofs << symbol->name() << " " << transitionTime << "\n"; prevTime = time; } } } ofs.close(); return true; } bool PoseSeq::exportSeqFileForFaceController(const std::string& filename) { LipSyncTranslator translator; translator.translatePoseSeq(*this); return translator.exportSeqFileForFaceController(filename); } PoseSeq::iterator PoseSeq::changeTime(iterator it, double newTime) { iterator newpos; iterator insertpos = seek(it, newTime, true); iterator nextpos = it; nextpos++; if(insertpos == it || insertpos == nextpos){ beginPoseModification(it); it->time_ = newTime; endPoseModification(it); newpos = it; } else { sigPoseRemoving_(it, true); PoseRef newRef(this, it->poseUnit(), newTime); newRef.setMaxTransitionTime(it->maxTransitionTime()); refs.erase(it); newpos = refs.insert(insertpos, newRef); sigPoseInserted_(newpos, true); } return newpos; } PoseUnitPtr PoseSeq::find(const std::string& name) { PoseUnitMap::iterator p = poseUnitMap.find(name); if(p != poseUnitMap.end()){ return p->second; } return PoseUnitPtr(); // null pointer } PoseSeq::iterator PoseSeq::seek(PoseSeq::iterator current, double time, bool seekPosToInsert) { if(current == refs.end()){ if(current == refs.begin()){ return current; } current--; } double ct = current->time(); if(ct == time){ if(seekPosToInsert){ current++; } } else if(ct > time){ while(current != refs.begin()){ current--; ct = current->time(); if(ct == time){ if(seekPosToInsert){ current++; } break; } else if(ct < time){ current++; break; } } } else { while(current != refs.end() && (current->time() < time)){ current++; } } return current; } PoseSeq::iterator PoseSeq::insert(PoseSeq::iterator current, double time, PoseUnitPtr poseUnit) { const string& name = poseUnit->name(); if(!name.empty()){ PoseUnitMap::iterator p = poseUnitMap.find(name); if(p != poseUnitMap.end()){ return insertSub(current, time, p->second); } else { poseUnitMap.insert(make_pair(name, poseUnit)); } } return insertSub(current, time, poseUnit); } PoseSeq::iterator PoseSeq::insertSub(PoseSeq::iterator current, double time, PoseUnitPtr poseUnit) { PoseRef ref(this, poseUnit, time); poseUnit->owner = this; poseUnit->seqLocalReferenceCounter++; return insert(current, time, ref); } PoseSeq::iterator PoseSeq::insert(iterator current, double time, const std::string& name) { if(name.empty()){ return refs.end(); } PoseUnitPtr punit = find(name); if(punit){ return insertSub(current, time, punit); } else { PoseRef ref(this, PoseUnitPtr(), time); return insert(current, time, ref); } } PoseSeq::iterator PoseSeq::insert(iterator current, double time, PoseRef& ref) { iterator it = seek(current, time); it = refs.insert(it, ref); sigPoseInserted_(it, false); return it; } PoseSeq::iterator PoseSeq::erase(iterator it) { sigPoseRemoving_(it, false); PoseUnitPtr unit = it->poseUnit(); if(unit){ unit->seqLocalReferenceCounter--; if(unit->seqLocalReferenceCounter == 0){ const string& name = unit->name(); if(!name.empty()){ poseUnitMap.erase(name); } unit->owner = 0; } } return refs.erase(it); } void PoseSeq::rename(iterator it, const std::string newName) { // unref current shared pose unit PoseUnitPtr currentPoseUnit = it->poseUnit(); if(currentPoseUnit){ const string& currentName = currentPoseUnit->name(); if(!currentName.empty()){ if(--currentPoseUnit->seqLocalReferenceCounter == 0){ poseUnitMap.erase(currentName); } } } PoseUnitPtr sharedPoseUnit = find(newName); if(sharedPoseUnit){ it->poseUnit_ = sharedPoseUnit; sharedPoseUnit->seqLocalReferenceCounter++; } else { if(currentPoseUnit){ PoseUnitPtr newUnit(currentPoseUnit->duplicate()); newUnit->name_ = newName; newUnit->owner = this; newUnit->seqLocalReferenceCounter++; it->poseUnit_ = newUnit; if(!newName.empty()){ poseUnitMap.insert(make_pair(newName, newUnit)); } } } } void PoseSeq::getDomain(double& out_lower, double& out_upper) { if(refs.empty()){ out_lower = 0.0; out_upper = 0.0; } else { out_lower = refs.front().time(); out_upper = refs.back().time(); } } ConnectionSet PoseSeq::connectSignalSet (const boost::signal::slot_type& slotInserted, const boost::signal::slot_type& slotRemoving, const boost::signal::slot_type& slotModified) { ConnectionSet connections; connections.add(sigPoseInserted_.connect(slotInserted)); connections.add(sigPoseRemoving_.connect(slotRemoving)); connections.add(sigPoseModified_.connect(slotModified)); return connections; } ConnectionSet PoseSeq::connectSignalSet (const boost::signal::slot_type& slotInserted, const boost::signal::slot_type& slotRemoving, const boost::signal::slot_type& slotModifying, const boost::signal::slot_type& slotModified) { ConnectionSet connections; connections.add(sigPoseInserted_.connect(slotInserted)); connections.add(sigPoseRemoving_.connect(slotRemoving)); connections.add(sigPoseModifying_.connect(slotModifying)); connections.add(sigPoseModified_.connect(slotModified)); return connections; } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeq.h000066400000000000000000000160771207742442300216560ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_CHOREOGRAPHY_POSE_SEQ_H_INCLUDED #define CNOID_CHOREOGRAPHY_POSE_SEQ_H_INCLUDED #include "Pose.h" #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class PoseSeq; typedef boost::intrusive_ptr PoseSeqPtr; class CNOID_EXPORT PoseRef { public: inline const std::string& name() const { return poseUnit_->name(); } inline const PoseUnitPtr poseUnit() const { return poseUnit_; } inline PoseUnitPtr poseUnit() { return poseUnit_; } template inline const boost::intrusive_ptr get() const { return boost::dynamic_pointer_cast(poseUnit_); } template inline boost::intrusive_ptr get() { return boost::dynamic_pointer_cast(poseUnit_); } inline double time() const { return time_; } /** This function sets the default transition time. When the time length between the previous pose and this pose is longer than the default transition time, the sysytem uses this time to interpolate the two poses. If the time length between the two poses is shorter than the default transition time, the former length is used for the interpolation. If the default transition time is zero, the interpolation is always done using the time length between the two poses. */ inline void setMaxTransitionTime(double time){ maxTransitionTime_ = time; } inline double maxTransitionTime() const { return maxTransitionTime_; } private: friend class PoseSeq; PoseRef(PoseSeq* owner, PoseUnitPtr poseUnit, double time); //PoseRef(const PoseRef& org, bool doDeepCopy = false); PoseSeq* owner; PoseUnitPtr poseUnit_; double time_; double maxTransitionTime_; }; class CNOID_EXPORT PoseSeq : public PoseUnit, public boost::signals::trackable { public: typedef std::list::iterator iterator; typedef std::list::const_iterator const_iterator; PoseSeq(); PoseSeq(const PoseSeq& org); ~PoseSeq(); void setName(const std::string& name); void setTargetBodyName(const std::string& name) { targetBodyName_ = name; } const std::string& targetBodyName() { return targetBodyName_; } iterator copyElement(iterator seekpos, const_iterator org, double offset = 0.0); virtual PoseUnit* duplicate(); bool load(const std::string& filename, const BodyPtr body); bool save(const std::string& filename, const BodyPtr body); bool exportTalkPluginFile(const std::string& filename); bool exportSeqFileForFaceController(const std::string& filename); const std::string& errorMessage() { return errorMessage_; } virtual bool restore(const YamlMapping& archive, const BodyPtr body); virtual void store(YamlMapping& archive, const BodyPtr body) const; iterator changeTime(iterator it, double time); inline bool empty() const { return refs.empty(); } inline std::list::size_type size() const { return refs.size(); } inline iterator begin(){ return refs.begin(); } inline const_iterator begin() const { return refs.begin(); } inline iterator end(){ return refs.end(); } inline const_iterator end() const { return refs.end(); } inline PoseRef& front() { return refs.front(); } inline PoseRef& back() { return refs.back(); } PoseUnitPtr find(const std::string& name); //iterator seek(double time, bool seekPosToInsert = false); iterator seek(iterator current, double time, bool seekPosToInsert = false); //iterator insert(double time, PoseUnitPtr pose); iterator insert(iterator current, double time, PoseUnitPtr pose); iterator insert(iterator current, double time, const std::string& name); iterator erase(iterator it); void rename(iterator it, const std::string newName); inline double beginningTime() { return refs.empty() ? 0.0 : refs.front().time(); } inline double endingTime() { return refs.empty() ? 0.0 : refs.back().time(); } void getDomain(double& out_lower, double& out_upper); //SignalProxy< boost::signal > sigNameChanged(); /* SignalProxy< boost::signal > sigPoseInserted() { return sigPoseInserted_; } SignalProxy< boost::signal > sigPoseRemoving(){ return sigPoseRemoving_; } SignalProxy< boost::signal > sigPoseModified(){ return sigPoseModified_; } */ /* void emitSigPoseModified(iterator it) { sigPoseModified_(it); } */ void beginPoseModification(iterator it){ sigPoseModifying_(it); } void endPoseModification(iterator it){ sigPoseModified_(it); } ConnectionSet connectSignalSet( const boost::signal::slot_type& slotInserted, const boost::signal::slot_type& slotRemoving, const boost::signal::slot_type& slotModified); ConnectionSet connectSignalSet( const boost::signal::slot_type& slotInserted, const boost::signal::slot_type& slotRemoving, const boost::signal::slot_type& slotModifying, const boost::signal::slot_type& slotModified); /// \todo Implement and use the followings. For example for loading a file. void blockSignals(); void unblockSignals(); private: typedef std::list PoseRefList; PoseRefList refs; typedef std::map PoseUnitMap; PoseUnitMap poseUnitMap; std::set storedNames; boost::signal sigPoseInserted_; boost::signal sigPoseRemoving_; boost::signal sigPoseModifying_; boost::signal sigPoseModified_; std::string targetBodyName_; std::string errorMessage_; iterator insertSub(PoseSeq::iterator current, double time, PoseUnitPtr poseUnit); iterator insert(iterator current, double time, PoseRef& ref); friend class PoseRef; }; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqEngine.cpp000066400000000000000000000055311207742442300233300ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "PoseSeqEngine.h" #include "PoseSeqItem.h" #include "BodyMotionGenerationBar.h" #include #include #include using namespace boost; using namespace cnoid; namespace { class PoseSeqEngine : public TimeSyncItemEngine { public: BodyItemPtr bodyItem; PoseSeqInterpolatorPtr interpolator; BodyMotionGenerationBar* bodyMotionGenerationBar; LinkTraverse fkTraverse; PoseSeqEngine(PoseSeqItem* poseSeqItem, BodyItem* bodyItem) : bodyItem(bodyItem) { interpolator = poseSeqItem->interpolator(); bodyMotionGenerationBar = BodyMotionGenerationBar::instance(); poseSeqItem->sigUpdated().connect(bind(&PoseSeqEngine::notifyUpdate, this)); interpolator->sigUpdated().connect(bind(&PoseSeqEngine::notifyUpdate, this)); } virtual bool onTimeChanged(double time){ BodyPtr body = bodyItem->body(); interpolator->enableLipSyncMix(bodyMotionGenerationBar->isLipSyncMixMode()); if(interpolator->interpolate(time)){ const int numJoints = body->numJoints(); for(int i=0; i < numJoints; ++i){ optional q = interpolator->jointPosition(i); if(q){ body->joint(i)->q = (*q); } } int baseLinkIndex = interpolator->baseLinkIndex(); if(baseLinkIndex >= 0){ Link* link = body->link(baseLinkIndex); interpolator->getBaseLinkPosition(link->p, link->R); if(link != fkTraverse.rootLink()){ fkTraverse.find(link, true, true); } fkTraverse.calcForwardKinematics(); } optional zmp = interpolator->zmp(); if(zmp){ bodyItem->setZmp(*zmp); } bodyItem->notifyKinematicStateChange(true); } return (time <= interpolator->endingTime()); } }; typedef boost::intrusive_ptr PoseSeqEnginePtr; TimeSyncItemEnginePtr createPoseSeqEngine(Item* sourceItem) { PoseSeqEnginePtr engine; PoseSeqItem* poseSeqItem = dynamic_cast(sourceItem); if(poseSeqItem){ BodyItem* bodyItem = poseSeqItem->findOwnerItem(); if(bodyItem){ engine = new PoseSeqEngine(poseSeqItem, bodyItem); } } return engine; } } void cnoid::initializePoseSeqEngine(ExtensionManager* em) { em->timeSyncItemEngineManger().addEngineFactory(createPoseSeqEngine); } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqEngine.h000066400000000000000000000004371207742442300227750ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #ifndef CNOID_CHOREOGRAPY_PLUGIN_POSE_SEQ_ENGINE_H_INCLUDED #define CNOID_CHOREOGRAPY_PLUGIN_POSE_SEQ_ENGINE_H_INCLUDED #include namespace cnoid { void initializePoseSeqEngine(ExtensionManager* em); } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqInterpolator.cpp000066400000000000000000002047051207742442300246110ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifdef _MSC_VER #define _USE_MATH_DEFINES #endif #include "PoseSeqInterpolator.h" #include "PronunSymbol.h" #include #include #include #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; const double epsilon = 1.0e-6; enum SegmentType { UNDETERMINED, INVALID, CUBIC_SPLINE, CUBIC_CONNECTION, MIN_JERK_CONNECTION, LINEAR, ZERO_LENGTH }; // coefficients for interpolatin struct Coeff { double y; // sample value double yp; // derivative value double a; double a_end; double b; double c; }; struct LinkSample { LinkSample(PoseSeq::iterator p, const Pose::LinkInfo& info){ poseIter = p; segmentType = UNDETERMINED; x = p->time(); isBaseLink = info.isBaseLink(); const Vector3& pos = info.p; const Vector3 rpy = rpyFromRot(info.R); for(int i=0; i < 3; ++i){ c[i].y = pos[i]; c[i].yp = 0.0; c[i+3].y = rpy[i]; c[i+3].yp = 0.0; } isTouching = info.isTouching(); isEndPoint = info.isStationaryPoint() || isTouching; isDirty = true; isSlave = info.isSlave() && !info.isTouching(); isAux = false; } void invalidateSegment(){ segmentType = INVALID; isEndPoint = true; } SegmentType segmentType; PoseSeq::iterator poseIter; double x; Coeff c[6]; // x, y, z, roll, pitch, yaw bool isBaseLink; bool isEndPoint; bool isDirty; bool isTouching; bool isSlave; bool isAux; typedef std::list Seq; }; struct LinkZSample { LinkZSample(PoseSeq::iterator p, const Pose::LinkInfo& info){ poseIter = p; segmentType = UNDETERMINED; x = p->time(); c[0].y = info.p[2]; c[0].yp = 0.0; isTouching = info.isTouching(); isEndPoint = info.isStationaryPoint() || isTouching; isDirty = true; } void invalidateSegment(){ segmentType = INVALID; isEndPoint = true; } SegmentType segmentType; PoseSeq::iterator poseIter; double x; Coeff c[1]; bool isEndPoint; bool isDirty; bool isTouching; typedef std::list Seq; }; struct LinkInfo { LinkInfo(BodyPtr& body, int linkIndex) { iter = samples.end(); zIter = zSamples.end(); Link* link = body->link(linkIndex); if(link){ jointId = link->jointId; } else { jointId = -1; } jointSpaceBlendingRatio = 0.0; isFootLink = false; } int jointId; bool isFootLink; LinkSample::Seq samples; LinkSample::Seq::iterator iter; LinkZSample::Seq zSamples; LinkZSample::Seq::iterator zIter; // interpolation state Vector3 p; Matrix3 R; bool isValid; double jointSpaceBlendingRatio; // 1.0 = joint space, 0.0 = Cartesian space }; struct JointSample { JointSample(PoseSeq::iterator p, int jointId, bool useLinearInterpolation) { poseIter = p; Pose* pose = static_cast(p->poseUnit().get()); if(useLinearInterpolation){ segmentType = LINEAR; isDirty = false; } else { segmentType = UNDETERMINED; isDirty = true; } x = p->time(); c[0].y = pose->jointPosition(jointId); c[0].yp = 0.0; isEndPoint = pose->isJointStationaryPoint(jointId); } SegmentType segmentType; PoseSeq::iterator poseIter; double x; Coeff c[1]; bool isEndPoint; bool isDirty; typedef std::list Seq; }; struct JointInfo { JointInfo(){ useLinearInterpolation = false; clear(); } void clear(){ samples.clear(); iter = samples.begin(); prevSegmentDirectionSign = 0.0; prev_q = 0.0; } JointSample::Seq samples; JointSample::Seq::iterator iter; bool useLinearInterpolation; double prevSegmentDirectionSign; double prev_q; // interpolated state optional q; }; struct ZmpSample { ZmpSample(PoseSeq::iterator p) { poseIter = p; Pose* pose = static_cast(p->poseUnit().get()); segmentType = UNDETERMINED; x = p->time(); const Vector3& zmp = pose->zmp(); for(int i=0; i < 3; ++i){ Coeff& ci = c[i]; ci.y = zmp[i]; ci.yp = 0.0; } isEndPoint = pose->isZmpStationaryPoint(); isDirty = true; } ZmpSample(double time, const Vector3& p){ segmentType = UNDETERMINED; x = time; for(int i=0; i < 3; ++i){ Coeff& ci = c[i]; ci.y = p[i]; ci.yp = 0.0; } isEndPoint = true; isDirty = true; } SegmentType segmentType; PoseSeq::iterator poseIter; double x; Coeff c[3]; bool isEndPoint; bool isDirty; typedef std::list Seq; }; } namespace cnoid { class PSIImpl { public: PSIImpl(PoseSeqInterpolator* self); PoseSeqInterpolator* self; BodyPtr body; PoseSeqPtr poseSeq; bool needUpdate; ConnectionSet poseSeqConnections; vector jointInfos; typedef map LinkInfoMap; LinkInfoMap ikLinkInfos; vector footLinkIndices; vector soleCenters; vector footLinkInfos; bool isAutoZmpAdjustmentMode; double minZmpTransitionTime; double zmpCenteringTimeThresh; double zmpTimeMarginBeforeLifting; bool isStealthyStepMode; double stealthyHeightRatioThresh; double flatLiftingHeight; double flatLandingHeight; double impactReductionHeight; double impactReductionTime; double impactReductionVelocity; ZmpSample::Seq zmpSamples; ZmpSample::Seq::iterator zmpIter; enum SupportPhase { RIGHT = 0, LEFT, BOTH, FLOATING, NONE }; enum LipShapeId { LS_A, LS_I, LS_U, LS_E, LS_O, LS_N, LS_a, LS_i, LS_u, LS_e, LS_o, NUM_LIP_SHAPES }; enum mixType { UPPER, LOWER, ADDITION }; struct LipSyncJoint { int jointId; int mixType; int orgIndex; }; struct LipSyncSample { double time; int shapeId; }; bool isLipSyncMixEnabled; vector lipSyncJoints; vector lipSyncLinkIndices; typedef multi_array ShapeArray; ShapeArray lipSyncShapes; vector lipSyncSeq; vector::iterator lipSyncIter; double lipSyncMaxTransitionTime; double currentTime; double timeScaleRatio; LinkInfoMap::iterator currentBaseLinkInfoIter; dynamic_bitset<> validIkLinkFlag; Vector3 waistTranslation; boost::signal sigUpdated; void setBody(BodyPtr body0); void setLinearInterpolationJoint(int jointId); void addFootLink(int linkIndex, const Vector3& soleCenter); void clearLipSyncShapes(); void setLipSyncShapes(const YamlMapping& lipSyncShapeNode); void setPoseSeq(PoseSeqPtr seq); void setStealthyStepParameters( double heightRatioThresh, double flatLiftingHeight, double flatLandingHeight, double impactReductionHeight, double impactReductionTime); void invalidateCurrentInterpolation(); bool interpolate(double time, int waistLinkIndex, const Vector3& waistTranslation); bool mixLipSyncShape(); void calcIkJointPositions(); void calcIkJointPositionsSub(Link* link, Link* baseLink, LinkInfo* baseLinkInfo, bool doUpward, Link* prevLink); void appendPronun(PoseSeq::iterator poseIter); void appendLinkSamples(PoseSeq::iterator poseIter, PosePtr& pose); void adjustZmpAndFootKeyPosesForLifting (LinkSample::Seq& swingSamples, LinkSample::Seq::iterator pSwing0, LinkSample::Seq::iterator pSwing1, LinkZSample::Seq& swingZSamples, LinkZSample::Seq::iterator pSwingZ0, LinkZSample::Seq::iterator pSwingZ1, ZmpSample::Seq::iterator pZmp0, const Vector3& zmpOnSupport, bool zmpCenteringDone); void adjustZmpAndFootKeyPosesForLanding (LinkSample::Seq& swingSamples, LinkSample::Seq::iterator pSwing0, LinkSample::Seq::iterator pSwing1, LinkZSample::Seq& swingZSamples, LinkZSample::Seq::iterator pSwingZ0, LinkZSample::Seq::iterator pSwingZ1, ZmpSample::Seq::iterator pZmp1, const Vector3& zmp1, const Vector3& zmpOnSupport); bool adjustZmpForBothPhase( ZmpSample::Seq::iterator& pZmp0, double time0, double time1, LinkSample::Seq::iterator pRight0, LinkZSample::Seq::iterator pRightZ0, LinkSample::Seq::iterator pLeft0, LinkZSample::Seq::iterator pLeftZ0, SupportPhase prevPhase, SupportPhase nextPhase); Vector3 getCenterZmp(const LinkSample::Seq::iterator& xyzrpy, const LinkZSample::Seq::iterator& z, int which); void adjustZmpAndFootKeyPoses(); void insertAuxKeyPosesForStealthySteps(); bool update(); LinkInfo* getIkLinkInfo(int linkIndex); void onPoseInserted(PoseSeq::iterator it); void onPoseRemoving(PoseSeq::iterator it, bool isMoving); void onPoseModified(PoseSeq::iterator it); }; } namespace { template typename SampleType::Seq::iterator updateZeroLengthSegment(typename SampleType::Seq::iterator s) { if(TRACE_FUNCTIONS){ cout << "updateZeroLengthSegment" << endl; } const typename SampleType::Seq::iterator s0 = s; const typename SampleType::Seq::iterator s1 = ++s; s0->segmentType = ZERO_LENGTH; s0->isDirty = false; s0->isEndPoint = true; s1->isEndPoint = true; return s; } template typename SampleType::Seq::iterator updateCubicConnectionSegment(typename SampleType::Seq::iterator s) { if(TRACE_FUNCTIONS){ cout << "updateCubicConnectionSegment" << endl; } const typename SampleType::Seq::iterator s0 = s; const typename SampleType::Seq::iterator s1 = ++s; if(fabs(s1->x - s0->x) < epsilon){ return updateZeroLengthSegment(s0); } s0->segmentType = CUBIC_CONNECTION; s0->isDirty = false; s0->isEndPoint = true; s1->isEndPoint = true; const double h = (s1->x - s0->x); const double h2 = h * h; const double h3 = h2 * h; for(int i=0; i < dim; ++i){ Coeff& c0 = s0->c[i]; Coeff& c1 = s1->c[i]; c0.a = c0.yp; c0.b = 3.0 * (c1.y - c0.y) / h2 - (2.0 * c0.yp + c1.yp) / h; c0.c = (c0.yp + c1.yp) / h2 + 2.0 * (c0.y - c1.y) / h3; } return s; } template typename SampleType::Seq::iterator updateMinJerkConnectionSegment(typename SampleType::Seq::iterator s) { const typename SampleType::Seq::iterator s0 = s; const typename SampleType::Seq::iterator s1 = ++s; s0->segmentType = MIN_JERK_CONNECTION; s0->isDirty = false; s0->isEndPoint = true; s1->isEndPoint = true; return s; } template typename SampleType::Seq::iterator updateCubicSplineSegment(typename SampleType::Seq::iterator s, typename SampleType::Seq::iterator end) { if(TRACE_FUNCTIONS){ cout << "updateCubicSplineSegment" << endl; } typename SampleType::Seq::iterator s0 = s; typename SampleType::Seq::iterator s1 = ++s; if(fabs(s1->x - s0->x) < epsilon){ return updateZeroLengthSegment(s0); } else { typename SampleType::Seq::iterator s2 = s1; ++s2; if(fabs(s2->x - s1->x) < epsilon){ return updateCubicConnectionSegment(s0); } } s0->segmentType = CUBIC_SPLINE; s0->isEndPoint = true; s0->isDirty = false; for(int i=0; i < dim; ++i){ Coeff& c0 = s0->c[i]; Coeff& c1 = s1->c[i]; c0.a = -0.5; c0.b = (3.0 / (s1->x - s0->x)) * ((c1.y - c0.y) / (s1->x - s0->x) - c0.yp); } while(true) { typename SampleType::Seq::iterator s0 = s; --s0; typename SampleType::Seq::iterator s1 = s; typename SampleType::Seq::iterator s2 = ++s; if(fabs(s2->x - s1->x) < epsilon){ --s; break; } s1->segmentType = CUBIC_SPLINE; s1->isDirty = false; double sig = (s1->x - s0->x) / (s2->x - s0->x); for(int i=0; i < dim; ++i){ Coeff& c0 = s0->c[i]; Coeff& c1 = s1->c[i]; Coeff& c2 = s2->c[i]; double p = sig * c0.a + 2.0; c1.a = (sig - 1.0) / p; double b = (c2.y - c1.y) / (s2->x - s1->x) - (c1.y - c0.y) / (s1->x - s0->x); c1.b = (6.0 * b / (s2->x - s0->x) - sig * c0.b) / p; } if(s2->isEndPoint || ++s2 == end){ break; } } typename SampleType::Seq::iterator sf0 = s; --sf0; typename SampleType::Seq::iterator sf = s; sf->isEndPoint = true; double a_save[dim]; for(int i=0; i < dim; ++i){ Coeff& cf0 = sf0->c[i]; Coeff& cf = sf->c[i]; double qf = 0.5; double bf = (3.0 / (sf->x - sf0->x)) * (cf.yp - (cf.y - cf0.y) / (sf->x - sf0->x)); a_save[i] = cf.a; cf.a = (bf - qf * cf0.b) / (qf * cf0.a + 1.0); } while(s != s0){ typename SampleType::Seq::iterator s1 = s; typename SampleType::Seq::iterator s0 = --s; for(int i=0; i < dim; ++i){ s0->c[i].a = s0->c[i].a * s1->c[i].a + s0->c[i].b; } } for(int i=0; i < dim; ++i){ sf->c[i].a_end = sf->c[i].a; sf->c[i].a = a_save[i]; } return sf; } /** pre-determined velocity version */ template void usePredeterminedVelocities(typename SampleType::Seq& samples) { typename SampleType::Seq::iterator s = samples.begin(); typename SampleType::Seq::iterator prev = s; while(s != samples.end()){ if(s->segmentType != INVALID){ typename SampleType::Seq::iterator next = s; ++next; while(true){ if(next == samples.end()){ next = s; break; } if(next->segmentType == INVALID){ ++next; } else { break; } } double dt0 = s->x - prev->x; double dt1 = next->x - s->x; for(int i=3; i < dim; ++i){ // for rotation over 180[deg] double& y = next->c[i].y; if(fabs(s->c[i].y - y) > M_PI){ double r = y - floor(y / (2.0 * M_PI)) * 2.0 * M_PI; y = floor(s->c[i].y / (2.0 * M_PI)) * 2.0 * M_PI + r; } } if(!s->isEndPoint){ for(int i=0; i < dim; ++i){ if(s->c[i].yp == 0.0){ double dy0 = (s->c[i].y - prev->c[i].y); double dy1 = (next->c[i].y - s->c[i].y); if(fabs(dy0) < 1.0e-3 || fabs(dy1) < 1.0e-3 || dy0 * dy1 <= 0.0){ s->c[i].yp = 0.0; } else { s->c[i].yp = (dy0 / dt0 + dy1 / dt1) / 2.0; } } } s->isEndPoint = true; } prev = s; } ++s; } } template void initializeInterpolation(typename SampleType::Seq& samples) { if(TRACE_FUNCTIONS){ cout << "initializeInterpolation" << endl; } usePredeterminedVelocities(samples); typename SampleType::Seq::iterator s = samples.begin(); while(s != samples.end()){ if(s->segmentType == INVALID){ ++s; } else { typename SampleType::Seq::iterator next = s; ++next; if(next == samples.end()){ break; } if(!next->isEndPoint && (++next != samples.end())){ s = updateCubicSplineSegment(s, samples.end()); } else { if(useJerkMinModel){ s = updateMinJerkConnectionSegment(s); } else { s = updateCubicConnectionSegment(s); } } } } } template bool interpolate( typename SampleType::Seq& samples, typename SampleType::Seq::iterator& p, double x, double* out_result) { if(samples.empty()){ return false; } if(p == samples.end()){ --p; if(p->x <= x){ for(int i=0; i < dim; ++i){ out_result[i] = p->c[i].y; } return true; } } typename SampleType::Seq::iterator next; while(true){ if(x < p->x){ if(p == samples.begin()){ for(int i=0; i < dim; ++i){ out_result[i] = p->c[i].y; } return true; } --p; continue; } while(true){ next = p; ++next; if(next == samples.end()){ for(int i=0; i < dim; ++i){ out_result[i] = p->c[i].y; } return true; } if(x < next->x){ break; } ++p; } break; } const SampleType& s0 = *p; const SampleType& s1 = *next; if(s0.isDirty){ return false; } switch(s0.segmentType){ case UNDETERMINED: case INVALID: return false; case CUBIC_SPLINE: for(int i=0; i < dim; ++i){ const double a_end = (s1.isEndPoint ? s1.c[i].a_end : s1.c[i].a); const double h = s1.x - s0.x; const double A = (s1.x - x) / h; const double B = (x - s0.x) / h; out_result[i] = A * s0.c[i].y + B * s1.c[i].y + ((A*A*A - A) * s0.c[i].a + (B*B*B - B) * a_end) * (h*h) / 6.0; } break; case CUBIC_CONNECTION: for(int i=0; i < dim; ++i){ const double& a0 = s0.c[i].y; const double& a1 = s0.c[i].a; const double& a2 = s0.c[i].b; const double& a3 = s0.c[i].c; const double h = x - s0.x; const double h2 = h * h; const double h3 = h2 * h; out_result[i] = (a0 + a1 * h + a2 * h2 + a3 * h3); } break; case MIN_JERK_CONNECTION: for(int i=0; i < dim; ++i){ double r = (x - s0.x) / (s1.x - s0.x); double r2 = r * r; double r3 = r2 * r; double r4 = r2 * r2; double r5 = r4 * r; double y0 = s0.c[i].y; double y1 = s1.c[i].y; out_result[i] = y0 + (y0 - y1) * (15.0 * r4 - 6.0 * r5 - 10.0 * r3); } break; case LINEAR: for(int i=0; i < dim; ++i){ double r = (x - s0.x) / (s1.x - s0.x); out_result[i] = (1.0 - r) * s0.c[i].y + r * s1.c[i].y; } break; case ZERO_LENGTH: for(int i=0; i < dim; ++i){ out_result[i] = s1.c[i].y; } break; } return true; } /** @return iterator to the last appended sample */ template typename SampleType::Seq::iterator applyMaxTransitionTime(typename SampleType::Seq& samples, const PoseSeq::iterator poseIter) { if(samples.empty()) return samples.end(); typename SampleType::Seq::iterator prev = --samples.end(); if(prev != samples.end()){ const double time = poseIter->time(); const double ttime = poseIter->maxTransitionTime(); if(ttime > 0.0 && time - prev->x > ttime){ prev->isEndPoint = true; samples.push_back(*prev); SampleType& sampleForTransition = samples.back(); sampleForTransition.x = time - ttime; sampleForTransition.isEndPoint = true; ++prev; } } return prev; } template void appendSample(typename SampleType::Seq& samples, const SampleType& sample) { applyMaxTransitionTime(samples, sample.poseIter); samples.push_back(sample); } } PoseSeqInterpolator::PoseSeqInterpolator() { impl = new PSIImpl(this); } PSIImpl::PSIImpl(PoseSeqInterpolator* self) : self(self) { timeScaleRatio = 1.0; isAutoZmpAdjustmentMode = false; minZmpTransitionTime = 0.1; zmpCenteringTimeThresh = 0.03; zmpTimeMarginBeforeLifting = 0.0; isStealthyStepMode = false; setStealthyStepParameters(2.0, 0.005, 0.005, 0.012, 0.3); isLipSyncMixEnabled = false; needUpdate = true; } void PoseSeqInterpolator::setBody(BodyPtr body) { impl->setBody(body); } void PSIImpl::setBody(BodyPtr body0) { if(!body0){ body = 0; } else { body = body0->duplicate(); int n = body->numJoints(); jointInfos.clear(); jointInfos.resize(n); ikLinkInfos.clear(); footLinkIndices.clear(); soleCenters.clear(); validIkLinkFlag.resize(body->numLinks()); clearLipSyncShapes(); invalidateCurrentInterpolation(); } needUpdate = true; } Body* PoseSeqInterpolator::body() const { return impl->body.get(); } void PoseSeqInterpolator::setLinearInterpolationJoint(int jointId) { impl->setLinearInterpolationJoint(jointId); } void PSIImpl::setLinearInterpolationJoint(int jointId) { if(jointId < (int)jointInfos.size()){ jointInfos[jointId].useLinearInterpolation = true; } } void PoseSeqInterpolator::addFootLink(int linkIndex, const Vector3& soleCenter) { impl->addFootLink(linkIndex, soleCenter); } void PSIImpl::addFootLink(int linkIndex, const Vector3& soleCenter) { footLinkIndices.push_back(linkIndex); soleCenters.push_back(soleCenter); needUpdate = true; } void PSIImpl::clearLipSyncShapes() { lipSyncJoints.clear(); lipSyncLinkIndices.clear(); lipSyncSeq.clear(); needUpdate = true; } void PoseSeqInterpolator::setLipSyncShapes(const YamlMapping& info) { impl->setLipSyncShapes(info); } /** \todo move this into LipSyncTranslator */ void PSIImpl::setLipSyncShapes(const YamlMapping& info) { needUpdate = true; clearLipSyncShapes(); if(!info.isValid()){ return; } const YamlSequence& jointSeq = *info["joints"].toSequence(); const YamlSequence& mixTypeSeq = *info["mixTypes"].toSequence(); int numOrgJoints = jointSeq.size(); for(int i=0; i < numOrgJoints; ++i){ Link* joint = body->link(jointSeq[i].toString()); if(joint){ LipSyncJoint lipSyncJoint; int mixType = -1; string mixTypeSymbol = mixTypeSeq[i].toString(); if(mixTypeSymbol == "upper"){ mixType = UPPER; } else if(mixTypeSymbol == "lower"){ mixType = LOWER; } else if(mixTypeSymbol == "addition"){ mixType = ADDITION; } if(mixType >= 0){ lipSyncJoint.jointId = joint->jointId; lipSyncJoint.mixType = mixType; lipSyncJoint.orgIndex = i; lipSyncJoints.push_back(lipSyncJoint); lipSyncLinkIndices.push_back(joint->index); } } } static const char* shapeSymbols[] = { "A", "I", "U", "E", "O", "N", "a", "i", "u", "e", "o" }; int numShapesRead = 0; if(!lipSyncJoints.empty()){ lipSyncShapes.resize(boost::extents[NUM_LIP_SHAPES][lipSyncJoints.size()]); const YamlMapping& shapes = *info["shapes"].toMapping(); for(int i=0; i < NUM_LIP_SHAPES; ++i){ const YamlSequence& shapeSeq = *shapes.findSequence(shapeSymbols[i]); if(!shapeSeq.isValid() || shapeSeq.size() != numOrgJoints){ break; } for(size_t j=0; j < lipSyncJoints.size(); ++j){ double q = radian(shapeSeq[lipSyncJoints[j].orgIndex].toDouble()); lipSyncShapes[i][j] = q; } numShapesRead++; } } lipSyncMaxTransitionTime = info.get("maxTransitionTime", 0.2); if(numShapesRead < NUM_LIP_SHAPES){ clearLipSyncShapes(); } } const std::vector& PoseSeqInterpolator::lipSyncLinkIndices() { return impl->lipSyncLinkIndices; } void PSIImpl::invalidateCurrentInterpolation() { currentTime = std::numeric_limits::max(); currentBaseLinkInfoIter = ikLinkInfos.end(); } void PoseSeqInterpolator::setPoseSeq(PoseSeqPtr seq) { impl->setPoseSeq(seq); } void PSIImpl::setPoseSeq(PoseSeqPtr seq) { poseSeqConnections.disconnect(); poseSeq = seq; // for auto update mode (not implemented yet) poseSeqConnections = seq->connectSignalSet( bind(&PSIImpl::onPoseInserted, this, _1), bind(&PSIImpl::onPoseRemoving, this, _1, _2), bind(&PSIImpl::onPoseModified, this, _1)); invalidateCurrentInterpolation(); needUpdate = true; } void PoseSeqInterpolator::setTimeScaleRatio(double ratio) { impl->timeScaleRatio = ratio; } double PoseSeqInterpolator::beginningTime() const { return 0.0; } double PoseSeqInterpolator::endingTime() const { if(impl->poseSeq){ return impl->timeScaleRatio * impl->poseSeq->endingTime(); } else { return 0.0; } } void PoseSeqInterpolator::enableAutoZmpAdjustmentMode(bool on) { impl->isAutoZmpAdjustmentMode = on; impl->needUpdate = true; } void PoseSeqInterpolator::setZmpAdjustmentParameters (double minTransitionTime, double centeringTimeThresh, double timeMarginBeforeLifting) { impl->minZmpTransitionTime = minTransitionTime; impl->zmpCenteringTimeThresh = centeringTimeThresh; impl->zmpTimeMarginBeforeLifting = timeMarginBeforeLifting; impl->needUpdate = true; } void PoseSeqInterpolator::enableStealthyStepMode(bool on) { impl->isStealthyStepMode = on; impl->needUpdate = true; } void PoseSeqInterpolator::setStealthyStepParameters (double heightRatioThresh, double flatLiftingHeight, double flatLandingHeight, double impactReductionHeight, double impactReductionTime) { impl->setStealthyStepParameters(heightRatioThresh, flatLiftingHeight, flatLandingHeight, impactReductionHeight, impactReductionTime); } void PSIImpl::setStealthyStepParameters (double heightRatioThresh, double flatLiftingHeight, double flatLandingHeight, double impactReductionHeight, double impactReductionTime) { this->stealthyHeightRatioThresh = heightRatioThresh; this->flatLiftingHeight = flatLiftingHeight; this->flatLandingHeight = flatLandingHeight; this->impactReductionHeight = impactReductionHeight; this->impactReductionTime = impactReductionTime; this->impactReductionVelocity = -2.0 * impactReductionHeight / impactReductionTime; needUpdate = true; } void PoseSeqInterpolator::enableLipSyncMix(bool on) { impl->isLipSyncMixEnabled = on; } bool PoseSeqInterpolator::update() { return impl->update(); } SignalProxy< boost::signal > PoseSeqInterpolator::sigUpdated() { return impl->sigUpdated; } bool PoseSeqInterpolator::interpolate(double time) { return impl->interpolate(time, -1, Vector3::Zero()); } bool PoseSeqInterpolator::seek(double time) { return impl->interpolate(time, -1, Vector3::Zero()); } /** @param waistLinkIndex A link that is translated for maintaing the dynamic balance @param waistTranslation translation of the balancing link usually provided by the waist balance filter */ bool PoseSeqInterpolator::interpolate(double time, int waistLinkIndex, const Vector3& waistTranslation) { return impl->interpolate(time, waistLinkIndex, waistTranslation); } /** A virtual function of the WaistTranslator interface */ bool PoseSeqInterpolator::seek(double time, int waistLinkIndex, const Vector3& waistTranslation) { return impl->interpolate(time, waistLinkIndex, waistTranslation); } /** \todo Skip interpolation when only waistTranslation changes */ bool PSIImpl::interpolate(double time, int waistLinkIndex, const Vector3& waistTranslation) { if(!body){ return false; } if(needUpdate){ if(!update()){ return false; } } time /= timeScaleRatio; // This should be applied to the process of appending samples if(time == currentTime && waistTranslation == this->waistTranslation){ return true; } currentTime = time; validIkLinkFlag.reset(); currentBaseLinkInfoIter = ikLinkInfos.end(); double baseLinkTime = -std::numeric_limits::max(); LinkInfoMap::iterator subBaseLinkInfoIter = ikLinkInfos.end(); double subBaseLinkTime = -std::numeric_limits::max(); bool waistTranslationDone = false; this->waistTranslation = waistTranslation; double xyzrpy[6]; for(LinkInfoMap::iterator it = ikLinkInfos.begin(); it != ikLinkInfos.end(); ++it){ LinkInfo& info = it->second; info.isValid = ::interpolate<6, LinkSample>(info.samples, info.iter, currentTime, xyzrpy); if(info.isValid){ /// \todo check this before the interpolation (after seeking only) /// \todo skip aux samples for detecting the blending segment info.jointSpaceBlendingRatio = 0.0; if(info.iter != info.samples.end()){ LinkSample::Seq::iterator prevNonAuxIter = info.iter; while(prevNonAuxIter->isAux){ --prevNonAuxIter; } double prevNonAuxTime = prevNonAuxIter->x; LinkSample::Seq::iterator nextIter = info.iter; nextIter++; LinkSample::Seq::iterator nextNonAuxIter = nextIter; while(nextNonAuxIter != info.samples.end() && nextNonAuxIter->isAux){ ++nextNonAuxIter; } if(prevNonAuxIter->isSlave){ if(nextNonAuxIter == info.samples.end() || nextNonAuxIter->isSlave){ info.jointSpaceBlendingRatio = 1.0; continue; // not Cartesian space at all ! skip to the next } else { double r; if(nextNonAuxIter->isTouching){ if(info.iter->isAux){ r =0.0; } else { r = (nextIter->x - currentTime) / (nextIter->x - prevNonAuxTime); } } else { r = (nextNonAuxIter->x - currentTime) / (nextNonAuxIter->x - prevNonAuxTime); } info.jointSpaceBlendingRatio = r * r * (3.0 - 2.0 * r); } } else { if(nextNonAuxIter != info.samples.end() && nextNonAuxIter->isSlave){ double r; if(prevNonAuxIter->isTouching){ if(nextIter == nextNonAuxIter){ r = (currentTime - info.iter->x) / (nextIter->x - info.iter->x); } else { r = 0.0; } } else { r = (currentTime - prevNonAuxTime) / (nextNonAuxIter->x - prevNonAuxTime); } info.jointSpaceBlendingRatio = r * r * (3.0 - 2.0 * r); } } } if(info.isFootLink){ ::interpolate<1, LinkZSample>(info.zSamples, info.zIter, currentTime, &xyzrpy[2]); } const Vector3 p(xyzrpy[0], xyzrpy[1], xyzrpy[2]); const int linkIndex = it->first; validIkLinkFlag[linkIndex] = true; if(linkIndex == waistLinkIndex){ info.p = p + waistTranslation; waistTranslationDone = true; } else { info.p = p + info.jointSpaceBlendingRatio * waistTranslation; } info.R = rotFromRpy(xyzrpy[3], xyzrpy[4], xyzrpy[5]); if(info.iter->isBaseLink){ if(info.iter->x > baseLinkTime){ currentBaseLinkInfoIter = it; baseLinkTime = info.iter->x; } } else { if(info.iter->x > subBaseLinkTime){ subBaseLinkInfoIter = it; subBaseLinkTime = info.iter->x; } } } } if(currentBaseLinkInfoIter == ikLinkInfos.end()){ currentBaseLinkInfoIter = subBaseLinkInfoIter; } for(size_t i=0; i < jointInfos.size(); ++i){ jointInfos[i].q = boost::none; } calcIkJointPositions(); if(waistLinkIndex >= 0 && !waistTranslationDone){ /// \todo write a code here to translate the waist when the waist link is not an ik link return false; } if(isLipSyncMixEnabled){ mixLipSyncShape(); } return true; } bool PSIImpl::mixLipSyncShape() { if(lipSyncSeq.empty()){ return false; } vector::iterator next = lipSyncSeq.end(); if(lipSyncIter == lipSyncSeq.end()){ --lipSyncIter; if(lipSyncIter->time <= currentTime){ goto mix; } } while(true){ if(currentTime < lipSyncIter->time){ if(lipSyncIter == lipSyncSeq.begin()){ goto mix; } --lipSyncIter; continue; } while(true){ next = lipSyncIter; ++next; if(next == lipSyncSeq.end()){ goto mix; } if(currentTime < next->time){ break; } ++lipSyncIter; } break; } mix: for(size_t i=0; i < lipSyncJoints.size(); ++i){ LipSyncJoint& lipSyncJoint = lipSyncJoints[i]; double q0 = lipSyncShapes[lipSyncIter->shapeId][i]; double q; if(next == lipSyncSeq.end()){ q = q0; } else { double q1 = lipSyncShapes[next->shapeId][i]; double t = (currentTime - lipSyncIter->time) / (next->time - lipSyncIter->time); q = (1.0 - t) * q0 + t * q1; } int jointId = lipSyncJoint.jointId; JointInfo& jointInfo = jointInfos[jointId]; optional qorg = self->jointPosition(jointId); if(!qorg){ jointInfo.q = q; } else { switch(lipSyncJoint.mixType){ case UPPER: jointInfo.q = std::max(*qorg, q); break; case LOWER: jointInfo.q = std::min(*qorg, q); break; case ADDITION: { q += *qorg; Link* joint = body->joint(jointId); jointInfo.q = std::max(joint->llimit, std::min(joint->ulimit, q)); } break; default: break; } } } return true; } void PSIImpl::calcIkJointPositions() { Link* baseLink; LinkInfo* baseLinkInfo; if(currentBaseLinkInfoIter != ikLinkInfos.end()){ baseLink = body->link(currentBaseLinkInfoIter->first); baseLinkInfo = ¤tBaseLinkInfoIter->second; } else { baseLink = body->rootLink(); baseLinkInfo = 0; } calcIkJointPositionsSub(baseLink, baseLink, baseLinkInfo, true, 0); } /** \todo search an analytical IK path even if the base link of the path is not an ik link */ void PSIImpl::calcIkJointPositionsSub(Link* link, Link* baseLink, LinkInfo* baseLinkInfo, bool doUpward, Link* prevLink) { if(link != baseLink && validIkLinkFlag[link->index]){ LinkInfo* endLinkInfo = getIkLinkInfo(link->index); if(baseLinkInfo && endLinkInfo){ JointPathPtr jointPath = body->getJointPath(baseLink, link); bool doIK = true; // tmp if(!jointPath->hasAnalyticalIK()){ // tmp if(jointPath->numJoints() != 6){ doIK = false; } for(int i=0; i < jointPath->numJoints(); ++i){ Link* joint = jointPath->joint(i); JointInfo& jointInfo = jointInfos[joint->jointId]; double q; if(::interpolate<1, JointSample>(jointInfo.samples, jointInfo.iter, currentTime, &q)){ jointInfo.q = q; joint->q = q; } } } if(doIK){ // tmp bool ikSolved = jointPath->calcInverseKinematics(baseLinkInfo->p, baseLinkInfo->R, endLinkInfo->p, endLinkInfo->R); if(!ikSolved){ double len = waistTranslation.norm(); double low = 0.0; double hi = len * (1.0 - endLinkInfo->jointSpaceBlendingRatio); while(true){ double current = (low + hi) / 2.0; Vector3 p(endLinkInfo->p + waistTranslation * (current / len)); if(jointPath->calcInverseKinematics(baseLinkInfo->p, baseLinkInfo->R, p, endLinkInfo->R)){ ikSolved = true; hi = current; } else { low = current; } if((hi - low) < 1.0e-4){ break; } } } if(ikSolved){ for(int i=0; i < jointPath->numJoints(); ++i){ Link* joint = jointPath->joint(i); JointInfo& jointInfo = jointInfos[joint->jointId]; const double& r = endLinkInfo->jointSpaceBlendingRatio; if(r == 0.0){ jointInfo.q = joint->q; } else { double qtmp; if(::interpolate<1, JointSample>(jointInfo.samples, jointInfo.iter, currentTime, &qtmp)){ jointInfo.q = r * qtmp + (1.0 - r) * joint->q; } else { jointInfo.q = joint->q; } } } } } } baseLink = link; baseLinkInfo = endLinkInfo; } if(doUpward && link->parent){ calcIkJointPositionsSub(link->parent, baseLink, baseLinkInfo, true, link); } for(Link* childLink = link->child; childLink; childLink = childLink->sibling){ if(childLink != prevLink){ calcIkJointPositionsSub(childLink, baseLink, baseLinkInfo, false, 0); } } } int PoseSeqInterpolator::baseLinkIndex() const { if(impl->currentBaseLinkInfoIter != impl->ikLinkInfos.end()){ return impl->currentBaseLinkInfoIter->first; } return -1; } bool PoseSeqInterpolator::getBaseLinkPosition(Vector3& out_p, Matrix3& out_R) const { if(impl->currentBaseLinkInfoIter != impl->ikLinkInfos.end()){ const LinkInfo& info = impl->currentBaseLinkInfoIter->second; out_p = info.p; out_R = info.R; return true; } return false; } boost::optional PoseSeqInterpolator::jointPosition(int jointId) const { JointInfo& info = impl->jointInfos[jointId]; if(!info.q){ double qtmp; if(::interpolate<1, JointSample>(info.samples, info.iter, impl->currentTime, &qtmp)){ info.q = qtmp; } } return info.q; } void PoseSeqInterpolator::getJointPositions(std::vector< boost::optional >& out_q) const { const int n = impl->jointInfos.size(); out_q.resize(n); for(int i=0; i < n; ++i){ out_q[i] = jointPosition(i); } } boost::optional PoseSeqInterpolator::zmp() const { Vector3 p; if(::interpolate<3, ZmpSample>(impl->zmpSamples, impl->zmpIter, impl->currentTime, p.data())){ return p; } return boost::none; } bool PSIImpl::update() { if(!body || !poseSeq){ return false; } for(size_t i=0; i < jointInfos.size(); ++i){ jointInfos[i].clear(); } ikLinkInfos.clear(); zmpSamples.clear(); lipSyncSeq.clear(); if(isAutoZmpAdjustmentMode || isStealthyStepMode){ footLinkInfos.clear(); for(size_t i=0; i < footLinkIndices.size(); ++i){ LinkInfo* info = getIkLinkInfo(footLinkIndices[i]); if(info){ info->isFootLink = true; footLinkInfos.push_back(info); } } } for(PoseSeq::iterator poseIter = poseSeq->begin(); poseIter != poseSeq->end(); ++poseIter){ PosePtr pose = poseIter->get(); if(!pose){ PronunSymbolPtr pronun = poseIter->get(); if(pronun){ appendPronun(poseIter); } } else { appendLinkSamples(poseIter, pose); const int n = std::min(pose->numJoints(), (int)jointInfos.size()); for(int i=0; i < n; ++i){ JointInfo& jointInfo = jointInfos[i]; if(pose->isJointValid(i)){ // make a flipping point stationary point double q = pose->jointPosition(i); double sign = q - jointInfo.prev_q; if(jointInfo.prevSegmentDirectionSign * sign <= 0.0){ if(!jointInfo.samples.empty()){ jointInfo.samples.back().isEndPoint = true; } } jointInfo.prevSegmentDirectionSign = sign; jointInfo.prev_q = q; appendSample(jointInfo.samples, JointSample(poseIter, i, jointInfo.useLinearInterpolation)); } } if(pose->isZmpValid()){ appendSample(zmpSamples, ZmpSample(poseIter)); } } } if(!footLinkInfos.empty()){ if(isAutoZmpAdjustmentMode && footLinkInfos.size() == 2){ adjustZmpAndFootKeyPoses(); } if(isStealthyStepMode){ insertAuxKeyPosesForStealthySteps(); } } for(size_t i=0; i < jointInfos.size(); ++i){ JointInfo& info = jointInfos[i]; if(TRACE_FUNCTIONS){ cout << "PSIImpl::update: joint " << i << endl; } if(!info.useLinearInterpolation){ initializeInterpolation<1, JointSample, false>(info.samples); } info.iter = info.samples.begin(); } for(LinkInfoMap::iterator p = ikLinkInfos.begin(); p != ikLinkInfos.end(); ++p){ LinkInfo& info = p->second; initializeInterpolation<6, LinkSample, false>(info.samples); info.iter = info.samples.begin(); if(info.isFootLink){ initializeInterpolation<1, LinkZSample, false>(info.zSamples); info.zIter = info.zSamples.begin(); } } initializeInterpolation<3, ZmpSample, false>(zmpSamples); zmpIter = zmpSamples.begin(); lipSyncIter = lipSyncSeq.begin(); invalidateCurrentInterpolation(); needUpdate = false; sigUpdated(); return true; } void PSIImpl::appendLinkSamples(PoseSeq::iterator poseIter, PosePtr& pose) { for(Pose::LinkInfoMap::iterator it = pose->ikLinkBegin(); it != pose->ikLinkEnd(); ++it){ const int linkIndex = it->first; LinkInfo* linkInfo = getIkLinkInfo(linkIndex); if(linkInfo){ const Pose::LinkInfo& ikLinkInfo = it->second; LinkSample::Seq& samples = linkInfo->samples; applyMaxTransitionTime(samples, poseIter); samples.push_back(LinkSample(poseIter, ikLinkInfo)); if(linkInfo->isFootLink){ LinkZSample::Seq& zSamples = linkInfo->zSamples; applyMaxTransitionTime(zSamples, poseIter); zSamples.push_back(LinkZSample(poseIter, ikLinkInfo)); } } } } static inline bool checkZmp(const Vector3& zmp, const Vector3& centerZmp) { return (zmp - centerZmp).squaredNorm() < (0.005 * 0.005); } void PSIImpl::adjustZmpAndFootKeyPosesForLifting (LinkSample::Seq& swingSamples, LinkSample::Seq::iterator pSwing0, LinkSample::Seq::iterator pSwing1, LinkZSample::Seq& swingZSamples, LinkZSample::Seq::iterator pSwingZ0, LinkZSample::Seq::iterator pSwingZ1, ZmpSample::Seq::iterator pZmp0, const Vector3& zmpOnSupport, bool zmpCenteringDone) { Vector3 zmp0(pZmp0->c[0].y, pZmp0->c[1].y, pZmp0->c[2].y); if(!checkZmp(zmp0, zmpOnSupport)){ double ttime0 = pSwing0->x - pZmp0->x; if(!zmpCenteringDone && ttime0 < minZmpTransitionTime){ double auxKeyTime = std::min(pZmp0->x + minZmpTransitionTime, (pSwing0->x + pSwing1->x) / 2.0); double zmpTime = std::max((pZmp0->x + auxKeyTime) / 2.0, auxKeyTime - zmpTimeMarginBeforeLifting); zmpSamples.insert(++pZmp0, ZmpSample(zmpTime, zmpOnSupport)); LinkSample::Seq::iterator pAux = swingSamples.insert(pSwing1, LinkSample(*pSwing0)); pAux->x = auxKeyTime; LinkZSample::Seq::iterator pZAux = swingZSamples.insert(pSwingZ1, LinkZSample(*pSwingZ0)); pZAux->x = auxKeyTime; } else { double zmpTime = std::max((pZmp0->x + pSwing0->x) / 2.0, pSwing0->x - zmpTimeMarginBeforeLifting); ZmpSample::Seq::iterator pAuxZmp = zmpSamples.insert(++pZmp0, ZmpSample(zmpTime, zmpOnSupport)); /* if(ttime0 >= minZmpTransitionTime * 2.0){ zmpSamples.insert(pAuxZmp, ZmpSample(pSwing0->x - minZmpTransitionTime * 2.0, zmp0)); } */ } } } void PSIImpl::adjustZmpAndFootKeyPosesForLanding (LinkSample::Seq& swingSamples, LinkSample::Seq::iterator pSwing0, LinkSample::Seq::iterator pSwing1, LinkZSample::Seq& swingZSamples, LinkZSample::Seq::iterator pSwingZ0, LinkZSample::Seq::iterator pSwingZ1, ZmpSample::Seq::iterator pZmp1, const Vector3& zmp1, const Vector3& zmpOnSupport) { if(!checkZmp(zmp1, zmpOnSupport)){ if(pZmp1 == zmpSamples.end()){ zmpSamples.insert(pZmp1, ZmpSample(pSwing1->x, zmpOnSupport)); } else { if((pZmp1->x - pSwing1->x) < minZmpTransitionTime){ double auxKeyTime = std::max(pZmp1->x - minZmpTransitionTime, (pSwing0->x + pSwing1->x) / 2.0); zmpSamples.insert(pZmp1, ZmpSample(auxKeyTime, zmpOnSupport)); LinkSample::Seq::iterator pAux = swingSamples.insert(pSwing1, LinkSample(*pSwing1)); pAux->x = auxKeyTime; LinkZSample::Seq::iterator pZAux = swingZSamples.insert(pSwingZ1, LinkZSample(*pSwingZ1)); pZAux->x = auxKeyTime; } else { zmpSamples.insert(pZmp1, ZmpSample(pSwing1->x, zmpOnSupport)); } } } } /** @return true if centering ZMP is inserted */ bool PSIImpl::adjustZmpForBothPhase (ZmpSample::Seq::iterator& pZmp0, double time0, double time1, LinkSample::Seq::iterator pRight0, LinkZSample::Seq::iterator pRightZ0, LinkSample::Seq::iterator pLeft0, LinkZSample::Seq::iterator pLeftZ0, SupportPhase prevPhase, SupportPhase nextPhase) { double len = time1 - time0; if(len < zmpCenteringTimeThresh){ return false; } Vector3 p0(pRight0->c[0].y, pRight0->c[1].y, pRightZ0->c[0].y); Matrix3 R0(rotFromRpy(pRight0->c[3].y, pRight0->c[4].y, pRight0->c[5].y)); Vector3 p1(pLeft0->c[0].y, pLeft0->c[1].y, pLeftZ0->c[0].y); Matrix3 R1(rotFromRpy(pLeft0->c[3].y, pLeft0->c[4].y, pLeft0->c[5].y)); double thresh = minZmpTransitionTime * 2.0; if(prevPhase != nextPhase){ thresh *= 3.0; } if(len > thresh){ Vector3 zmp = (p0 + R0 * soleCenters[0] + p1 + R1 * soleCenters[1]) / 2.0; zmp[2] = 0.0; zmpSamples.insert(++pZmp0, ZmpSample(time0 + minZmpTransitionTime, zmp)); pZmp0 = zmpSamples.insert(pZmp0, ZmpSample(time1 - minZmpTransitionTime, zmp)); } else if(prevPhase == nextPhase){ double r = 0.5; double thresh2 = (2.0 * minZmpTransitionTime) * 0.6; if(len < thresh2){ r = 0.5 * (len / thresh2); if(prevPhase == RIGHT){ r = 1.0 - r; } } Vector3 zmp = (p0 + R0 * soleCenters[0]) * r + (p1 + R1 * soleCenters[1]) * (1.0 - r); zmp[2] = 0.0; pZmp0 = zmpSamples.insert(++pZmp0, ZmpSample(time0 + len / 2.0, zmp)); } return true; } Vector3 PSIImpl::getCenterZmp (const LinkSample::Seq::iterator& xyzrpy, const LinkZSample::Seq::iterator& z, int which) { const Vector3 p(xyzrpy->c[0].y, xyzrpy->c[1].y, z->c[0].y); const Matrix3 R = rotFromRpy(xyzrpy->c[3].y, xyzrpy->c[4].y, xyzrpy->c[5].y); Vector3 zmp = p + R * soleCenters[which]; zmp[2] = 0.0; return zmp; } void PSIImpl::adjustZmpAndFootKeyPoses() { // actual left and right may be exchanged LinkSample::Seq& leftSamples = footLinkInfos[LEFT]->samples; LinkSample::Seq& rightSamples = footLinkInfos[RIGHT]->samples; LinkZSample::Seq& leftZSamples = footLinkInfos[LEFT]->zSamples; LinkZSample::Seq& rightZSamples = footLinkInfos[RIGHT]->zSamples; if(leftSamples.empty() || rightSamples.empty()){ return; } LinkSample::Seq::iterator pLeft0, pLeft, pLeftNext, pRight0, pRight, pRightNext; LinkZSample::Seq::iterator pLeftZ0, pLeftZ, pLeftZNext, pRightZ0, pRightZ, pRightZNext; pLeft = pLeftNext = leftSamples.begin(); pRight = pRightNext = rightSamples.begin(); pLeftZ = pLeftZNext = leftZSamples.begin(); pRightZ = pRightZNext = rightZSamples.begin(); // insert initial zmp if(zmpSamples.empty()){ Vector3 zmp0 = Vector3::Zero(); int n = 0; if(pLeft->isTouching){ zmp0 += getCenterZmp(pLeft, pLeftZ, LEFT); n++; } if(pRight->isTouching){ zmp0 += getCenterZmp(pRight, pRightZ, RIGHT); n++; } if(n >0 ){ zmp0 /= n; } zmpSamples.push_back(ZmpSample(0.0, zmp0)); } ZmpSample::Seq::iterator pZmp0 = zmpSamples.begin(); bool isLeftTouching = true; bool isRightTouching = true; SupportPhase prevPhase = NONE; double time = 0.0; double time0 = 0.0; SupportPhase phaseBeforeBothPhase = NONE; ZmpSample::Seq::iterator pZmpInBothPhase = zmpSamples.end(); double bothPhaseTime0 = 0.0; bool doContinue = true; while(doContinue){ if(pLeftNext != leftSamples.end()){ if(pRightNext != rightSamples.end()){ if(pLeftNext->x < pRightNext->x){ pLeft = pLeftNext++; pLeftZ = pLeftZNext++; isLeftTouching = pLeft->isTouching; time = pLeft->x; } else if(pLeftNext->x == pRightNext->x){ pLeft = pLeftNext++; pLeftZ = pLeftZNext++; isLeftTouching = pLeft->isTouching; pRight = pRightNext++; pRightZ = pRightZNext++; isRightTouching = pRight->isTouching; time = pLeft->x; } else { pRight = pRightNext++; pRightZ = pRightZNext++; isRightTouching = pRight->isTouching; time = pRight->x; } } else { pLeft = pLeftNext++; pLeftZ = pLeftZNext++; isLeftTouching = pLeft->isTouching; time = pLeft->x; } } else if(pRightNext != rightSamples.end()){ pRight = pRightNext++; pRightZ = pRightZNext++; isRightTouching = pRight->isTouching; time = pRight->x; } else { doContinue = false; } SupportPhase phase; if(!doContinue){ phase = NONE; } else { if(isLeftTouching){ if(isRightTouching){ phase = BOTH; } else { phase = LEFT; } } else { if(isRightTouching){ phase = RIGHT; } else { phase = FLOATING; } } } while(++pZmp0 != zmpSamples.end()){ if(pZmp0->x > time0){ break; } } --pZmp0; if(prevPhase == BOTH){ if(pZmp0->x > bothPhaseTime0){ pZmpInBothPhase = pZmp0; } if(phase == LEFT || phase == RIGHT){ bool zmpCenteringDone = false; if(pZmpInBothPhase == zmpSamples.end()){ zmpCenteringDone = adjustZmpForBothPhase( pZmp0, bothPhaseTime0, time0, pRight0, pRightZ0, pLeft0, pLeftZ0, phaseBeforeBothPhase, phase); } if(phase == LEFT){ adjustZmpAndFootKeyPosesForLifting( rightSamples, pRight0, pRight, rightZSamples, pRightZ0, pRightZ, pZmp0, getCenterZmp(pLeft, pLeftZ, LEFT), zmpCenteringDone); } else if(phase == RIGHT){ adjustZmpAndFootKeyPosesForLifting( leftSamples, pLeft0, pLeft, leftZSamples, pLeftZ0, pLeftZ, pZmp0, getCenterZmp(pRight, pRightZ, RIGHT), zmpCenteringDone); } } } else if(phase == BOTH){ if(prevPhase == LEFT || prevPhase == RIGHT){ ZmpSample::Seq::iterator pZmp1 = pZmp0; while(pZmp1 != zmpSamples.end() && pZmp1->x < time){ ++pZmp1; } Vector3 zmp1; if(pZmp1 != zmpSamples.end() && pZmp1->x == time){ zmp1 << pZmp1->c[0].y, pZmp1->c[1].y, pZmp1->c[2].y; } else { //ZmpSample::Seq::iterator pZmpLast = --zmpSamples.end(); //zmp1 = pZmpLast->c[0].y, pZmpLast->c[1].y, pZmpLast->c[2].y; zmp1.fill(numeric_limits::max()); } if(prevPhase == LEFT){ adjustZmpAndFootKeyPosesForLanding( rightSamples, pRight0, pRight, rightZSamples, pRightZ0, pRightZ, pZmp1, zmp1, getCenterZmp(pLeft, pLeftZ, LEFT)); } else if(prevPhase == RIGHT){ adjustZmpAndFootKeyPosesForLanding( leftSamples, pLeft0, pLeft, leftZSamples, pLeftZ0, pLeftZ, pZmp1, zmp1, getCenterZmp(pRight, pRightZ, RIGHT)); } phaseBeforeBothPhase = prevPhase; bothPhaseTime0 = time; pZmpInBothPhase = zmpSamples.end(); } } prevPhase = phase; time0 = time; pRight0 = pRight; pLeft0 = pLeft; pRightZ0 = pRightZ; pLeftZ0 = pLeftZ; } // insert last ZMP if(++pZmp0 == zmpSamples.end()){ Vector3 zmpf = Vector3::Zero(); int n = 0; if(pLeft->isTouching){ zmpf += getCenterZmp(pLeft, pLeftZ, LEFT); n++; } if(pRight->isTouching){ zmpf += getCenterZmp(pRight, pRightZ, RIGHT); n++; } if(n >0 ){ zmpf /= n; } zmpSamples.push_back(ZmpSample(time + minZmpTransitionTime, zmpf)); } } /* void PSIImpl::insertAuxKeyPosesForStealthySteps() { for(size_t i=0; i < footLinkInfos.size(); ++i){ LinkInfo* linkInfo = footLinkInfos[i]; LinkSample::Seq& samples = linkInfo->samples; LinkZSample::Seq& zSamples = linkInfo->zSamples; if(!samples.empty()){ LinkSample::Seq::iterator pprev = samples.begin(); LinkSample::Seq::iterator p = pprev; ++p; LinkZSample::Seq::iterator pprevZ = zSamples.begin(); LinkZSample::Seq::iterator pZ = pprevZ; ++pZ; while(p != samples.end()){ if(pprev->isTouching && !p->isTouching){ // lifting double height = pZ->c[0].y - pprevZ->c[0].y; double sheight; if(height >= 4.0 * stealthyHeight){ sheight = stealthyHeight; } else { sheight = height / 4.0; } LinkSample::Seq::iterator paux = samples.insert(p, *pprev); paux->x += (sheight / height) * (p->x - pprev->x); } else if(!pprev->isTouching && p->isTouching){ // landing double touchingHeight = pZ->c[0].y; double height = pprevZ->c[0].y - touchingHeight; if(!isStealthyOnlyRotation){ double r = 1.0; double time = p->x - pprev->x; if(stealthyMaxRatio * time < stealthyTime){ r = (stealthyMaxRatio * time) / stealthyTime; } if(stealthyMaxRatio * height < stealthyHeight){ r = std::min(r, (stealthyMaxRatio * height) / stealthyHeight); } double t = r * stealthyTime; double h = r * stealthyHeight; double v = 2.0 * h / t; LinkSample::Seq::iterator paux = samples.insert(p, LinkSample(*p)); paux->isAux = true; paux->x -= t; LinkZSample::Seq::iterator pZAux = zSamples.insert(pZ, LinkZSample(*pZ)); pZAux->x -= t; pZAux->c[0].y = touchingHeight + h; pZAux->c[0].yp = -v; } else { if(stealthyMaxRatio * height > stealthyHeight){ LinkSample::Seq::iterator paux = samples.insert(p, LinkSample(*p)); paux->x -= (stealthyHeight / height) * (p->x - pprev->x); } } } pprev = p++; pprevZ = pZ++; } } } } */ void PSIImpl::insertAuxKeyPosesForStealthySteps() { for(size_t i=0; i < footLinkInfos.size(); ++i){ LinkInfo* linkInfo = footLinkInfos[i]; LinkSample::Seq& samples = linkInfo->samples; LinkZSample::Seq& zSamples = linkInfo->zSamples; if(!samples.empty()){ LinkSample::Seq::iterator pprev = samples.begin(); LinkSample::Seq::iterator p = pprev; ++p; LinkZSample::Seq::iterator pprevZ = zSamples.begin(); LinkZSample::Seq::iterator pZ = pprevZ; ++pZ; while(p != samples.end()){ if(pprev->isTouching && !p->isTouching){ // lifting if(flatLiftingHeight > 0.0){ double height = pZ->c[0].y - pprevZ->c[0].y; if(height >= stealthyHeightRatioThresh * flatLiftingHeight){ LinkSample::Seq::iterator paux = samples.insert(p, *pprev); paux->x += (flatLiftingHeight / height) * (p->x - pprev->x); } } } else if(!pprev->isTouching && p->isTouching){ // landing if(flatLandingHeight > 0.0){ double touchingHeight = pZ->c[0].y; double height = pprevZ->c[0].y - touchingHeight; if(height >= stealthyHeightRatioThresh * flatLandingHeight){ LinkSample::Seq::iterator paux = samples.insert(p, LinkSample(*p)); const double fallingTime = p->x - pprev->x; paux->isAux = true; paux->x -= (flatLandingHeight / height) * fallingTime; if(impactReductionHeight > 0.0 && impactReductionTime < fallingTime / 2.0){ const double h = fallingTime; const double h2 = h * h; const double h3 = h2 * h; const double a2 = 3.0 * (pZ->c[0].y - pprevZ->c[0].y) / h2; const double a3 = 2.0 * (pprevZ->c[0].y - pZ->c[0].y) / h3; const double s = fallingTime - impactReductionTime; const double v = 2.0 * a2 * s + 3.0 * a3 * s * s; if(v < impactReductionVelocity){ LinkZSample::Seq::iterator pZaux = zSamples.insert(pZ, LinkZSample(*pZ)); pZaux->x -= impactReductionTime; pZaux->c[0].y += impactReductionHeight; pZaux->c[0].yp = impactReductionVelocity; } } } } } pprev = p++; pprevZ = pZ++; } } } } void PSIImpl::appendPronun(PoseSeq::iterator poseIter) { const string& pronun = poseIter->name(); if(pronun.empty()){ return; } int vowel = -1; switch(tolower(pronun[pronun.size()-1])){ case 'a': vowel = LS_A; break; case 'i': vowel = LS_I; break; case 'u': vowel = LS_U; break; case 'e': vowel = LS_E; break; case 'o': vowel = LS_O; break; case 'n': vowel = LS_N; break; case ',': vowel = LS_N; break; case '.': vowel = LS_N; break; default: break; } if(vowel < 0){ return; } LipSyncSample sample0; sample0.shapeId = -1; LipSyncSample sample1; sample1.shapeId = -1; if(vowel != LS_N && pronun.size() >= 2){ int consonantChar = tolower(pronun[0]); if(consonantChar == 'm' || consonantChar == 'b' || consonantChar == 'p'){ sample0.shapeId = LS_N; } else { if(!lipSyncSeq.empty()){ int prevVowel = lipSyncSeq.back().shapeId; if(vowel == prevVowel){ sample0.shapeId = vowel + LS_a; } } } } if(sample0.shapeId < 0){ sample0.shapeId = vowel; } else { sample1.shapeId = vowel; } double time = poseIter->time(); while(!lipSyncSeq.empty()){ double prevTime = lipSyncSeq.back().time; double ttime = time - prevTime; if(ttime <= 0.0){ lipSyncSeq.pop_back(); continue; } if(ttime > lipSyncMaxTransitionTime){ lipSyncSeq.push_back(lipSyncSeq.back()); lipSyncSeq.back().time = time - lipSyncMaxTransitionTime; } break; } sample0.time = time; lipSyncSeq.push_back(sample0); if(sample1.shapeId >= 0){ sample1.time = time + 0.05; lipSyncSeq.push_back(sample1); } } LinkInfo* PSIImpl::getIkLinkInfo(int linkIndex) { LinkInfoMap::iterator p = ikLinkInfos.find(linkIndex); if(p == ikLinkInfos.end()){ if(linkIndex >= 0 && linkIndex < body->numLinks()){ p = ikLinkInfos.insert(make_pair(linkIndex, LinkInfo(body, linkIndex))).first; } else { return 0; } } return &p->second; } void PSIImpl::onPoseInserted(PoseSeq::iterator it) { needUpdate = true; } void PSIImpl::onPoseRemoving(PoseSeq::iterator it, bool isMoving) { needUpdate = true; } void PSIImpl::onPoseModified(PoseSeq::iterator it) { needUpdate = true; } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqInterpolator.h000066400000000000000000000047331207742442300242550ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_CHOREOGRAPHY_POSE_SEQ_INTERPOLATOR_H_INCLUDED #define CNOID_CHOREOGRAPHY_POSE_SEQ_INTERPOLATOR_H_INCLUDED #include "PoseSeq.h" #include #include #include #include "exportdecl.h" namespace cnoid { class YamlMapping; class PSIImpl; class CNOID_EXPORT PoseSeqInterpolator : public PoseProvider { public: PoseSeqInterpolator(); void setBody(BodyPtr body); Body* body() const; void setLinearInterpolationJoint(int jointId); void addFootLink(int linkIndex, const Vector3& soleCenter); void setLipSyncShapes(const YamlMapping& info); const std::vector& lipSyncLinkIndices(); void setPoseSeq(PoseSeqPtr seq); void setTimeScaleRatio(double ratio); double beginningTime() const; double endingTime() const; void enableStealthyStepMode(bool on); void setStealthyStepParameters( double heightRatioThresh, double flatLiftingHeight, double flatLandingHeight, double impactReductionHeight, double impactReductionTime); void enableAutoZmpAdjustmentMode(bool on); void setZmpAdjustmentParameters( double minTransitionTime, double centeringTimeThresh, double timeMarginBeforeLifting); void enableLipSyncMix(bool on); /** This function has not been implemented yet. */ void setAutoUpdateMode(bool on); bool update(); SignalProxy< boost::signal > sigUpdated(); bool interpolate(double time); bool interpolate(double time, int waistLinkIndex, const Vector3& waistTranslation); virtual bool seek(double time); virtual bool seek(double time, int waistLinkIndex, const Vector3& waistTranslation); /** @return -1 if base link is not set for the time segment */ int baseLinkIndex() const; bool getBaseLinkPosition(Vector3& out_p, Matrix3& out_R) const; boost::optional jointPosition(int jointId) const; boost::optional zmp() const; virtual void getJointPositions(std::vector< boost::optional >& out_q) const; private: PSIImpl* impl; }; typedef boost::shared_ptr PoseSeqInterpolatorPtr; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqItem.cpp000066400000000000000000000457161207742442300230320ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "PoseSeqItem.h" #include "BodyMotionGenerationBar.h" #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; bool loadPoseSeqItem(PoseSeqItem* item, const std::string& filename, std::ostream& os, Item* parentItem) { bool loaded = false; BodyItem* bodyItem = parentItem->findOwnerItem(true); if(bodyItem){ item->clearEditHistory(); loaded = item->poseSeq()->load(filename, bodyItem->body()); if(loaded){ const string& name = item->poseSeq()->name(); if(!name.empty()){ item->setName(name); } if(item->poseSeq()->targetBodyName() != bodyItem->body()->name()){ format m(_("Warning: the original target body %1% of \"%2%\" is" "different from the current target %3%.")); os << str(m % item->poseSeq()->targetBodyName() % item->name() % bodyItem->body()->name()); } item->notifyUpdate(); } else { os << item->poseSeq()->errorMessage(); } } else { os << _("PoseSeqItem must be loaded as a child of a BodyItem"); } return loaded; } bool savePoseSeqItem(PoseSeqItem* item, const std::string& filename, std::ostream& os, Item* parentItem) { BodyItem* bodyItem = parentItem->findOwnerItem(true); if(bodyItem){ return item->poseSeq()->save(filename, bodyItem->body()); } else { os << "PoseSeqItem to save must be a child of a BodyItem "; } return false; } bool exportTalkPluginFormat(PoseSeqItem* item, const std::string& filename) { return item->poseSeq()->exportTalkPluginFile(filename); } bool exportFaceControllerFormat(PoseSeqItem* item, const std::string& filename) { return item->poseSeq()->exportSeqFileForFaceController(filename); } } void PoseSeqItem::initializeClass(ExtensionManager* ext) { static bool initialized = false; if(!initialized){ ItemManager& im = ext->itemManager(); im.registerClass(N_("PoseSeqItem")); im.addCreationPanel(); im.addLoaderAndSaver( _("Pose Sequence"), "POSE-SEQ-YAML", "pseq", loadPoseSeqItem, savePoseSeqItem); im.addSaver( _("Talk Plugin File"), "TALK-PLUGIN-FORMAT", "talk", bind(exportTalkPluginFormat, _1, _2), ItemManager::PRIORITY_CONVERSION); im.addSaver( _("Seq File for the Face Controller"), "FACE-CONTROLLER-SEQ-FORMAT", "poseseq", bind(exportFaceControllerFormat, _1, _2), ItemManager::PRIORITY_CONVERSION); initialized = true; } } PoseSeqItem::PoseSeqItem() : seq(new PoseSeq()) { init(); } PoseSeqItem::PoseSeqItem(const PoseSeqItem& org) : Item(org), seq(new PoseSeq(*org.seq)) { init(); } void PoseSeqItem::init() { ownerBodyItem = 0; interpolator_.reset(new PoseSeqInterpolator()); interpolator_->setPoseSeq(seq); bodyMotionItem_ = new BodyMotionItem(); bodyMotionItem_->setName("motion"); addSubItem(bodyMotionItem_); clearEditHistory(); sigPositionChanged().connect(bind(&PoseSeqItem::onPositionChanged, this)); generationBar = BodyMotionGenerationBar::instance(); isSelectedPoseMoving = false; } PoseSeqItem::~PoseSeqItem() { editConnections.disconnect(); sigInterpolationParametersChangedConnection.disconnect(); } void PoseSeqItem::setName(const std::string& name) { seq->setName(name); setInconsistencyWithLastAccessedFile(); Item::setName(name); } void PoseSeqItem::onPositionChanged() { if(!sigInterpolationParametersChangedConnection.connected()){ sigInterpolationParametersChangedConnection = BodyMotionGenerationBar::instance()->sigInterpolationParametersChanged().connect( bind(&PoseSeqItem::updateInterpolationParameters, this)); updateInterpolationParameters(); } BodyItem* prevOwnerBodyItem = ownerBodyItem; ownerBodyItem = findOwnerItem(); if(ownerBodyItem){ BodyPtr body = ownerBodyItem->body(); if(seq->targetBodyName().empty()){ seq->setTargetBodyName(body->name()); } else if(prevOwnerBodyItem && (seq->targetBodyName() != body->name())){ convert(prevOwnerBodyItem->body()); } interpolator_->setBody(body); const YamlSequence& linearInterpolationJoints = *ownerBodyItem->body()->info()->findSequence("linearInterpolationJoints"); if(linearInterpolationJoints.isValid()){ for(int i=0; i < linearInterpolationJoints.size(); ++i){ Link* link = body->link(linearInterpolationJoints[i].toString()); if(link){ interpolator_->setLinearInterpolationJoint(link->jointId); } } } LeggedBody* legged = dynamic_cast(ownerBodyItem->body()); if(legged){ for(int i=0; i < legged->numFeet(); ++i){ const LeggedBody::FootInfo& info = legged->footInfo(i); interpolator_->addFootLink(info.link->index, info.soleCenter); } } interpolator_->setLipSyncShapes(*ownerBodyItem->body()->info()->findMapping("lipSyncShapes")); bodyMotionItem_->motion()->setNumParts(interpolator_->body()->numJoints()); bodyMotionItem_->notifyUpdate(); } else { interpolator_->setBody(0); } } void PoseSeqItem::convert(BodyPtr orgBody) { if(!orgBody){ return; } const YamlSequence& convInfoTop = *ownerBodyItem->body()->info()->findSequence("poseConversionInfo"); if(convInfoTop.isValid()){ for(int i=0; i < convInfoTop.size(); ++i){ const YamlMapping& convInfo = *convInfoTop[i].toMapping(); const YamlSequence& targets = *convInfo["targetBodies"].toSequence(); for(int j=0; j < targets.size(); ++j){ if(targets[j].toString() == orgBody->name()){ beginEditing(); if(endEditing(convertSub(orgBody, convInfo))){ clearLastAccessInformation(); BodyPtr body = ownerBodyItem->body(); seq->setTargetBodyName(body->name()); format m(_("Pose seq \"%1%\" has been converted. Its target has been changed from %2% to %3%")); MessageView::mainInstance()->notify(str(m % name() % orgBody->name() % body->name())); return; } } } } } } bool PoseSeqItem::convertSub(BodyPtr orgBody, const YamlMapping& convInfo) { bool converted = false; const YamlSequence& jointMap = *convInfo.findSequence("jointMap"); const YamlMapping& linkMap = *convInfo.findMapping("linkMap"); BodyPtr body = ownerBodyItem->body(); for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){ PosePtr pose = p->get(); if(pose){ bool modified = false; seq->beginPoseModification(p); PosePtr orgPose = dynamic_cast(pose->duplicate()); if(jointMap.isValid()){ modified = true; pose->setNumJoints(0); int n = orgPose->numJoints(); for(int i=0; i < n; ++i){ if(orgPose->isJointValid(i)){ if(i < jointMap.size()){ int newJointId = jointMap[i].toInt(); if(newJointId >= 0){ pose->setJointPosition(newJointId, orgPose->jointPosition(i)); pose->setJointStationaryPoint(newJointId, orgPose->isJointStationaryPoint(i)); } } } } } if(linkMap.isValid()){ modified = true; pose->clearIkLinks(); int baseLinkIndex = -1; for(Pose::LinkInfoMap::const_iterator q = orgPose->ikLinkBegin(); q != orgPose->ikLinkEnd(); ++q){ Link* orgLink = orgBody->link(q->first); string linkName; YamlNode* linkNameNode = linkMap.find(orgLink->name()); if(linkNameNode->isValid()){ linkName = linkNameNode->toString(); } else { linkName = orgLink->name(); } Link* link = body->link(linkName); if(link){ const Pose::LinkInfo& orgLinkInfo = q->second; Pose::LinkInfo* linkInfo = pose->addIkLink(link->index); linkInfo->p = orgLinkInfo.p; linkInfo->R = orgLinkInfo.R; linkInfo->setStationaryPoint(orgLinkInfo.isStationaryPoint()); if(orgLinkInfo.isTouching()){ linkInfo->setTouching(orgLinkInfo.partingDirection()); } linkInfo->setSlave(orgLinkInfo.isSlave()); if(orgLinkInfo.isBaseLink()){ baseLinkIndex = link->index; } } } if(baseLinkIndex >= 0){ pose->setBaseLink(baseLinkIndex); } } if(modified){ seq->endPoseModification(p); converted = true; } } } return converted; } ItemPtr PoseSeqItem::doDuplicate() const { return new PoseSeqItem(*this); } void PoseSeqItem::updateInterpolationParameters() { interpolator_->setTimeScaleRatio(generationBar->timeScaleRatio()); interpolator_->enableStealthyStepMode(generationBar->isStealthyStepMode()); interpolator_->setStealthyStepParameters( generationBar->stealthyHeightRatioThresh(), generationBar->flatLiftingHeight(), generationBar->flatLandingHeight(), generationBar->impactReductionHeight(), generationBar->impactReductionTime()); interpolator_->enableAutoZmpAdjustmentMode(generationBar->isAutoZmpAdjustmentMode()); interpolator_->setZmpAdjustmentParameters( generationBar->minZmpTransitionTime(), generationBar->zmpCenteringTimeThresh(), generationBar->zmpTimeMarginBeforeLifting()); interpolator_->enableLipSyncMix(generationBar->isLipSyncMixMode()); } bool PoseSeqItem::updateInterpolation() { updateInterpolationParameters(); return interpolator_->update(); } bool PoseSeqItem::updateTrajectory(bool putMessages) { bool result = false; if(ownerBodyItem){ result = generationBar->shapeBodyMotion( ownerBodyItem->body(), interpolator_.get(), bodyMotionItem_, putMessages); } return result; } void PoseSeqItem::beginEditing() { if(TRACE_FUNCTIONS){ cout << "PoseSeqItem::beginEditing()" << endl; } newHistory.clear(); inserted.clear(); modified.clear(); modifyingPoseIter = seq->end(); if(editConnections.empty()){ editConnections = seq->connectSignalSet( bind(&PoseSeqItem::onInserted, this, _1, _2), bind(&PoseSeqItem::onRemoving, this, _1, _2), bind(&PoseSeqItem::onModifying, this, _1), bind(&PoseSeqItem::onModified, this, _1)); } } bool PoseSeqItem::endEditing(bool actuallyModified) { if(TRACE_FUNCTIONS){ cout << "PoseSeqItem::endEditing()" << endl; } if(actuallyModified){ for(std::set::iterator p = inserted.begin(); p != inserted.end(); ++p){ newHistory.added->insert(newHistory.added->end(), (*p)->time(), (*p)->poseUnit()->duplicate()) ->setMaxTransitionTime((*p)->maxTransitionTime()); } for(std::set::iterator p = modified.begin(); p != modified.end(); ++p){ newHistory.added->insert(newHistory.added->end(), (*p)->time(), (*p)->poseUnit()->duplicate()) ->setMaxTransitionTime((*p)->maxTransitionTime()); } if(!newHistory.empty()){ editHistories.resize(currentHistory); editHistories.push_back(newHistory); currentHistory = editHistories.size(); setInconsistencyWithLastAccessedFile(); } } modifyingPoseIter = seq->end(); inserted.clear(); modified.clear(); newHistory.clear(); editConnections.disconnect(); return actuallyModified; } void PoseSeqItem::onInserted(PoseSeq::iterator p, bool isMoving) { if(isSelectedPoseMoving && isMoving){ modified.insert(p); isSelectedPoseMoving = false; } inserted.insert(p); } void PoseSeqItem::onRemoving(PoseSeq::iterator p, bool isMoving) { if(isMoving){ if(modified.find(p) != modified.end()){ modified.erase(p); isSelectedPoseMoving = true; } } if(inserted.find(p) != inserted.end()){ inserted.erase(p); } else { newHistory.removed->insert(newHistory.removed->end(), p->time(), p->poseUnit()->duplicate()) ->setMaxTransitionTime(p->maxTransitionTime()); } } void PoseSeqItem::onModifying(PoseSeq::iterator p) { if(TRACE_FUNCTIONS){ cout << "PoseSeqItem::onModifying()" << endl; } modifyingPoseTime = p->time(); modifyingPoseTTime = p->maxTransitionTime(); modifyingPoseUnitOrg = p->poseUnit()->duplicate(); modifyingPoseIter = p; } void PoseSeqItem::onModified(PoseSeq::iterator p) { if(TRACE_FUNCTIONS){ cout << "PoseSeqItem::onModified()" << endl; } if(p == modifyingPoseIter){ if(modified.find(p) == modified.end()){ newHistory.removed->insert(newHistory.removed->end(), modifyingPoseTime, modifyingPoseUnitOrg) ->setMaxTransitionTime(modifyingPoseTTime); modified.insert(p); } } modifyingPoseIter = seq->end(); } void PoseSeqItem::clearEditHistory() { currentHistory = 0; editHistories.clear(); } PoseSeq::iterator PoseSeqItem::removeSameElement(PoseSeq::iterator current, PoseSeq::iterator p) { current = seq->seek(current, p->time()); while(current->time() == p->time()){ if(current->poseUnit()->hasSameParts(p->poseUnit())){ return seq->erase(current); } ++current; } return current; } bool PoseSeqItem::undo() { if(currentHistory > 0){ editConnections.block(); EditHistory& history = editHistories[--currentHistory]; PoseSeqPtr added = history.added; PoseSeq::iterator current = seq->begin(); for(PoseSeq::iterator p = added->begin(); p != added->end(); ++p){ current = removeSameElement(current, p); } PoseSeqPtr removed = history.removed; for(PoseSeq::iterator p = removed->begin(); p != removed->end(); ++p){ current = seq->insert(current, p->time(), p->poseUnit()->duplicate()); current->setMaxTransitionTime(p->maxTransitionTime()); } editConnections.unblock(); setInconsistencyWithLastAccessedFile(); return true; } return false; } bool PoseSeqItem::redo() { if(currentHistory < (int)editHistories.size()){ editConnections.block(); EditHistory& history = editHistories[currentHistory++]; PoseSeqPtr removed = history.removed; PoseSeq::iterator current = seq->begin(); for(PoseSeq::iterator p = removed->begin(); p != removed->end(); ++p){ current = removeSameElement(current, p); } PoseSeqPtr added = history.added; for(PoseSeq::iterator p = added->begin(); p != added->end(); ++p){ current = seq->insert(current, p->time(), p->poseUnit()->duplicate()); current->setMaxTransitionTime(p->maxTransitionTime()); } editConnections.unblock(); setInconsistencyWithLastAccessedFile(); return true; } return false; } bool PoseSeqItem::updateKeyPosesWithBalancedTrajectories(std::ostream& os) { BodyMotionPtr motion = bodyMotionItem()->motion(); MultiValueSeqPtr qseq = motion->jointPosSeq(); MultiAffine3SeqPtr pseq = motion->linkPosSeq(); double length = seq->endingTime(); if(qseq->timeLength() < length || pseq->timeLength() < length){ os << "Length of the interpolated trajectories is shorter than key pose sequence."; return false; } if(pseq->numParts() < ownerBodyItem->body()->numLinks()){ os << "Not all link positions are available. Please do interpolate with \"Put all link positions\""; return false; } beginEditing(); for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){ PosePtr pose = p->get(); if(pose){ seq->beginPoseModification(p); int nj = pose->numJoints(); int frame = qseq->frameOfTime(p->time()); MultiValueSeq::View q = qseq->frame(frame); for(int i=0; i < nj; ++i){ if(pose->isJointValid(i)){ pose->setJointPosition(i, q[i]); } } MultiAffine3Seq::View pos = pseq->frame(frame); for(Pose::LinkInfoMap::iterator q = pose->ikLinkBegin(); q != pose->ikLinkEnd(); ++q){ int linkIndex = q->first; Pose::LinkInfo& info = q->second; const Vector3& p = pos[linkIndex].translation(); // only update horizontal position info.p[0] = p[0]; info.p[1] = p[1]; } seq->endPoseModification(p); } } endEditing(); updateInterpolation(); return true; } void PoseSeqItem::doPutProperties(PutPropertyFunction& putProperty) { putProperty(_("targetBody"), seq->targetBodyName()); } bool PoseSeqItem::store(Archive& archive) { if(overwrite()){ archive.writeRelocatablePath("filename", lastAccessedFileName()); archive.write("format", lastAccessedFileFormatId()); return true; } return false; } bool PoseSeqItem::restore(const Archive& archive) { std::string filename, formatId; if(archive.readRelocatablePath("filename", filename) && archive.read("format", formatId)){ if(load(filename, archive.currentParentItem(), formatId)){ return true; } } return false; } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqItem.h000066400000000000000000000075241207742442300224720ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #ifndef CNOID_CHOREOGRAPHY_BODY_POSE_SEQ_ITEM_H_INCLUDED #define CNOID_CHOREOGRAPHY_BODY_POSE_SEQ_ITEM_H_INCLUDED #include "PoseSeq.h" #include "PoseSeqInterpolator.h" #include #include #include #include "exportdecl.h" namespace cnoid { class TimeBar; class BodyItem; class BodyMotionGenerationBar; class CNOID_EXPORT PoseSeqItem : public cnoid::Item { public: static void initializeClass(ExtensionManager* ext); PoseSeqItem(); PoseSeqItem(const PoseSeqItem& org); ~PoseSeqItem(); virtual void setName(const std::string& name); inline PoseSeqPtr poseSeq() { return seq; } inline PoseSeqInterpolatorPtr interpolator(){ return interpolator_; } inline BodyMotionItem* bodyMotionItem(){ return bodyMotionItem_.get(); } virtual bool updateInterpolation(); virtual bool updateTrajectory(bool putMessages = false); void beginEditing(); bool endEditing(bool actuallyModified = true); void clearEditHistory(); bool undo(); bool redo(); /** temporary treatment. */ bool updateKeyPosesWithBalancedTrajectories(std::ostream& os); protected: virtual ItemPtr doDuplicate() const; virtual void doPutProperties(PutPropertyFunction& putProperty); virtual bool store(Archive& archive); virtual bool restore(const Archive& archive); BodyItem* ownerBodyItem; PoseSeqPtr seq; PoseSeqInterpolatorPtr interpolator_; BodyMotionItemPtr bodyMotionItem_; boost::signals::connection sigInterpolationParametersChangedConnection; ConnectionSet editConnections; struct EditHistory { /* Unify these containers into one which contains elements in the operated orders and restore them in the same order when undo or redo is carried out. */ PoseSeqPtr removed; PoseSeqPtr added; EditHistory(){ removed = new PoseSeq(); added = new PoseSeq(); } bool empty(){ return removed->empty() && added->empty(); } void clear(){ if(!empty()){ removed = new PoseSeq(); added = new PoseSeq(); } } }; struct PoseIterComp { bool operator()(const PoseSeq::iterator it1, const PoseSeq::iterator it2) const { return &(*it1) < &(*it2); } }; std::set inserted; std::set modified; double modifyingPoseTime; double modifyingPoseTTime; PoseUnitPtr modifyingPoseUnitOrg; PoseSeq::iterator modifyingPoseIter; std::deque editHistories; EditHistory newHistory; int currentHistory; BodyMotionGenerationBar* generationBar; TimeBar* timeBar; bool isSelectedPoseMoving; void init(); void convert(BodyPtr orgBody); bool convertSub(BodyPtr orgBody, const YamlMapping& convInfo); void updateInterpolationParameters(); void onInserted(PoseSeq::iterator p, bool isMoving); void onRemoving(PoseSeq::iterator p, bool isMoving); void onModifying(PoseSeq::iterator p); void onModified(PoseSeq::iterator p); PoseSeq::iterator removeSameElement(PoseSeq::iterator current, PoseSeq::iterator p); void onPositionChanged(); }; typedef boost::intrusive_ptr PoseSeqItemPtr; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqPlugin.cpp000066400000000000000000000027061207742442300233620ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include #include #include #include "PoseSeqItem.h" #include "PoseSeqEngine.h" #include "BodyMotionGenerationBar.h" //#include "PoseSeqView.h" #include "PoseRollView.h" #include "FcpFileLoader.h" #include #include "gettext.h" using namespace cnoid; namespace { class PoseSeqPlugin : public Plugin { public: PoseSeqPlugin() : Plugin("PoseSeq") { require("Body"); addOldName("Choreography"); } virtual bool initialize(){ PoseSeqItem::initializeClass(this); initializePoseSeqEngine(this); BodyMotionGenerationBar::initializeInstance(this); //addView(new PoseSeqView(*this)); addView(new PoseRollView()); initializeFcpFileLoader(*this); return true; } virtual const char* description() { static std::string text = str(boost::format(_("PoseSeq Plugin Version %1%\n")) % CNOID_FULL_VERSION_STRING) + "\n" + _("This plugin has been developed by Shin'ichiro Nakaoka and Choreonoid Development Team, AIST, " "and is distributed as a part of the Choreonoid package.\n" "\n") + LGPLtext(); return text.c_str(); } }; } CNOID_IMPLEMENT_PLUGIN_ENTRY(PoseSeqPlugin); choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqPlugin.qrc000066400000000000000000000003301207742442300233540ustar00rootroot00000000000000 icons/trajectory-generation.png icons/auto-update.png icons/balancer.png choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqView.cpp000066400000000000000000000732241207742442300230410ustar00rootroot00000000000000/** \file \author Shin'ichiro Nakaoka */ #include "PoseSeqView.h" #include "PoseSeqViewBase.h" #include "PronunSymbol.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include using namespace std; using namespace Glib; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; inline double degree(double rad) { return (180.0 * rad / 3.14159265358979); } inline double radian(double deg) { return (3.14159265358979 * deg / 180.0); } inline optional convertTextToDouble(const Glib::ustring& text) { optional v; try { v = lexical_cast(text); } catch(const boost::bad_lexical_cast& ){ } return v; } class MoveOperationDialog : public Gtk::Dialog { public: MoveOperationDialog(); Gtk::Label timeSpinLabel; Gtk::SpinButton timeSpin; }; } namespace cnoid { class PoseSeqViewImpl : public PoseSeqViewBase { public: PoseSeqViewImpl(PoseSeqView* self, ExtensionManager& ext); ~PoseSeqViewImpl(); PoseSeqView* self; Gtk::VPaned vpaned; Gtk::HBox hSplitBox; Gtk::VSeparator separator; Gtk::RadioMenuItem* verticalSplitRadio; Gtk::RadioMenuItem* horizontalSplitRadio; Gtk::ScrolledWindow swLinkTreeWidget; Gtk::ScrolledWindow swPoseListView; Gtk::TreeView poseListView; Gtk::TreeViewColumn* timeViewColumn; Gtk::TreeViewColumn* labelViewColumn; Gtk::TreeViewColumn* actualPronunPoseViewColumn; Gtk::TreeViewColumn* transitionTimeViewColumn; Gtk::TreeModelColumn poseIterColumn; Gtk::TreeModel::ColumnRecord record; Glib::RefPtr listStore; Glib::RefPtr listSelection; sigc::connection listSelectionChangedConnection; Gtk::TreeIter tmpLastIter; typedef std::map PoseRefToRowIterMap; PoseRefToRowIterMap poseRefToRowIterMap; Gtk::RadioMenuItem* normalModeRadio; Gtk::RadioMenuItem* humanoidModeRadio; Gtk::RadioMenuItem* lipsyncModeRadio; bool isLipSyncMode; Gtk::TreeModel::Path pressedPath; bool isPressedPathValid; Gtk::Menu popupMenu; MoveOperationDialog moveOperationDialog; void setupMainMenu(ExtensionManager& ext); void setupPopupMenu(); void layoutOperationParts(); void setupPoseListView(); void updatePlacementOfPoseListViewAndLinkTreeView(); void onModeMenuItemToggled(Gtk::RadioMenuItem* menuItem); void onModeToggled(); void onSplitModeMenuItemToggled(Gtk::RadioMenuItem* menuItem); virtual void setCurrentPoseSeqItem(PoseSeqItemPtr poseSeqItem); void resetPoseListViewModel(); void nameCellDataFunction(Gtk::CellRenderer* cell, const Gtk::TreeModel::iterator& iter); void actualPronunPoseCellDataFunction(Gtk::CellRenderer* cell, const Gtk::TreeModel::iterator& iter); void timeCellDataFunction( Gtk::CellRenderer* cell, const Gtk::TreeModel::iterator& iter); void transitionTimeCellDataFunction( Gtk::CellRenderer* cell, const Gtk::TreeModel::iterator& iter); virtual void onPoseInserted(PoseSeq::iterator it, bool isMoving); virtual void onPoseRemoving(PoseSeq::iterator it, bool isMoving); bool getSelectedPoseIters(vector& iters); void onListSelectionChanged(); virtual bool onTimeChanged(double time); virtual void onInsertPoseButtonClicked(); void onPoseTimeEdited(const Glib::ustring& path, const Glib::ustring& text); void onPoseNameEdited(const Glib::ustring& path, const Glib::ustring& text); void onPoseTransitionTimeEdited(const Glib::ustring& path, const Glib::ustring& text); bool onPoseListViewKeyPressEvent(GdkEventKey* event); bool onPoseListViewButtonPressEvent(GdkEventButton* event); void onCutActivated(); void onCopyActivated(); void onPasteActivated(); void onMoveActivated(); Gtk::TreeIter getListLastIter(); bool getListLastIterCallback(const Gtk::TreeModel::iterator& iter); bool restoreState(const Archive& archive); bool storeState(Archive& archive); }; } PoseSeqView::PoseSeqView(ExtensionManager& ext) { impl = new PoseSeqViewImpl(this, ext); } PoseSeqViewImpl::PoseSeqViewImpl(PoseSeqView* self, ExtensionManager& ext) : PoseSeqViewBase(self), self(self) { self->set_name(N_("Pose Seq")); self->setDefaultLayoutArea(View::CENTER); setupMainMenu(ext); setupPopupMenu(); layoutOperationParts(); setupPoseListView(); swLinkTreeWidget.set_policy(Gtk::POLICY_NEVER, Gtk::POLICY_ALWAYS); swLinkTreeWidget.add(linkTreeWidget); swLinkTreeWidget.show(); mainVBox.pack_start(hSplitBox, Gtk::PACK_SHRINK); mainVBox.pack_start(vpaned); updatePlacementOfPoseListViewAndLinkTreeView(); onModeToggled(); } void PoseSeqViewImpl::setupMainMenu(ExtensionManager& ext) { MenuManager& mm = ext.menuManager(); mm.setPath("/Options").setPath(N_("Pose Seq View")); mm.requestNewRadioGroup(); normalModeRadio = mm.addRadioItem(_("Normal mode")); normalModeRadio->set_active(true); normalModeRadio->signal_toggled().connect( sigc::bind(sigc::mem_fun(*this, &PoseSeqViewImpl::onModeMenuItemToggled), normalModeRadio)); humanoidModeRadio = mm.addRadioItem(_("Humanoid mode")); humanoidModeRadio->signal_toggled().connect( sigc::bind(sigc::mem_fun(*this, &PoseSeqViewImpl::onModeMenuItemToggled), humanoidModeRadio)); lipsyncModeRadio = mm.addRadioItem(_("Lip-sync mode")); lipsyncModeRadio->signal_toggled().connect( sigc::bind(sigc::mem_fun(*this, &PoseSeqViewImpl::onModeMenuItemToggled), lipsyncModeRadio)); mm.addSeparator(); mm.requestNewRadioGroup(); verticalSplitRadio = mm.addRadioItem(_("Vertical split mode")); verticalSplitRadio->set_active(true); verticalSplitRadio->signal_toggled().connect( sigc::bind(sigc::mem_fun(*this, &PoseSeqViewImpl::onSplitModeMenuItemToggled), verticalSplitRadio)); horizontalSplitRadio = mm.addRadioItem(_("Horizontal split mode")); horizontalSplitRadio->signal_toggled().connect( sigc::bind(sigc::mem_fun(*this, &PoseSeqViewImpl::onSplitModeMenuItemToggled), horizontalSplitRadio)); } void PoseSeqViewImpl::setupPopupMenu() { Gtk::Menu::MenuList& menulist = popupMenu.items(); menulist.push_back( Gtk::Menu_Helpers::MenuElem(_("_Cut"), sigc::mem_fun(*this, &PoseSeqViewImpl::onCutActivated))); menulist.push_back( Gtk::Menu_Helpers::MenuElem(_("_Copy"), sigc::mem_fun(*this, &PoseSeqViewImpl::onCopyActivated))); menulist.push_back( Gtk::Menu_Helpers::MenuElem(_("_Paste"), sigc::mem_fun(*this, &PoseSeqViewImpl::onPasteActivated))); menulist.push_back( Gtk::Menu_Helpers::MenuElem(_("_Move"), sigc::mem_fun(*this, &PoseSeqViewImpl::onMoveActivated))); } void PoseSeqViewImpl::layoutOperationParts() { Gtk::HBox* hbox = Gtk::manage(new Gtk::HBox()); hbox->set_border_width(3); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("Target Data: "))), Gtk::PACK_SHRINK); hbox->pack_start(currentItemLabel); mainVBox.pack_start(*hbox, Gtk::PACK_SHRINK); hbox = Gtk::manage(new Gtk::HBox()); hbox->pack_start(insertPoseButton, Gtk::PACK_SHRINK); hbox->pack_start(*Gtk::manage(new Gtk::Label(_("t.time"))), Gtk::PACK_SHRINK); hbox->pack_start(transitionTimeSpin, Gtk::PACK_SHRINK); hbox->pack_start(updateButton, Gtk::PACK_SHRINK); hbox->pack_start(timeSyncCheck, Gtk::PACK_SHRINK); mainVBox.pack_start(*hbox, Gtk::PACK_SHRINK); mainVBox.show_all_children(); } void PoseSeqViewImpl::setupPoseListView() { record.add(poseIterColumn); int columnIndex = 0; Gtk::CellRendererSpin* timeCell = Gtk::manage(new Gtk::CellRendererSpin()); poseListView.append_column(_("time"), *timeCell); timeViewColumn = poseListView.get_column(columnIndex++); timeViewColumn->set_cell_data_func( *timeCell, sigc::mem_fun(*this, &PoseSeqViewImpl::timeCellDataFunction)); timeCell->property_adjustment() = new Gtk::Adjustment(0.0, 0.0, 999.99, 0.01, 0.1, 0); timeCell->property_digits() = 2; timeCell->property_editable() = true; timeCell->signal_edited().connect (sigc::mem_fun(*this, &PoseSeqViewImpl::onPoseTimeEdited)); Gtk::CellRendererText* labelCell = Gtk::manage(new Gtk::CellRendererText()); poseListView.append_column(_("label"), *labelCell); labelViewColumn = poseListView.get_column(columnIndex++); labelViewColumn->set_expand(true); labelViewColumn->set_cell_data_func( *labelCell, sigc::mem_fun(*this, &PoseSeqViewImpl::nameCellDataFunction)); labelCell->signal_edited().connect(sigc::mem_fun(*this, &PoseSeqViewImpl::onPoseNameEdited)); Gtk::CellRendererText* pronunCell = Gtk::manage(new Gtk::CellRendererText()); poseListView.append_column(_("actual pose"), *pronunCell); actualPronunPoseViewColumn = poseListView.get_column(columnIndex++); actualPronunPoseViewColumn->set_cell_data_func( *pronunCell, sigc::mem_fun(*this, &PoseSeqViewImpl::actualPronunPoseCellDataFunction)); Gtk::CellRendererSpin* transitionTimeCell = Gtk::manage(new Gtk::CellRendererSpin()); poseListView.append_column(_("t.time"), *transitionTimeCell); transitionTimeViewColumn = poseListView.get_column(columnIndex++); transitionTimeViewColumn->set_cell_data_func( *transitionTimeCell, sigc::mem_fun(*this, &PoseSeqViewImpl::transitionTimeCellDataFunction)); transitionTimeCell->property_adjustment() = new Gtk::Adjustment(0.0, 0.0, 999.99, 0.01, 0.1, 0); transitionTimeCell->property_digits() = 2; transitionTimeCell->signal_edited().connect (sigc::mem_fun(*this, &PoseSeqViewImpl::onPoseTransitionTimeEdited)); //listStore = Gtk::ListStore::create(record); //poseListView.set_model(listStore); poseListView.set_grid_lines(Gtk::TREE_VIEW_GRID_LINES_HORIZONTAL); poseListView.signal_key_press_event().connect( sigc::mem_fun(*this, &PoseSeqViewImpl::onPoseListViewKeyPressEvent)); poseListView.signal_button_press_event().connect( sigc::mem_fun(*this, &PoseSeqViewImpl::onPoseListViewButtonPressEvent), false); listSelection = poseListView.get_selection(); listSelection->set_mode(Gtk::SELECTION_MULTIPLE); listSelectionChangedConnection = listSelection->signal_changed().connect (sigc::mem_fun(*this, &PoseSeqViewImpl::onListSelectionChanged)); swPoseListView.set_policy(Gtk::POLICY_NEVER, Gtk::POLICY_ALWAYS); swPoseListView.add(poseListView); poseListView.show(); swPoseListView.show(); } void PoseSeqViewImpl::updatePlacementOfPoseListViewAndLinkTreeView() { if(verticalSplitRadio->get_active()){ hSplitBox.hide(); if(swPoseListView.get_parent() == &hSplitBox){ hSplitBox.remove(swPoseListView); hSplitBox.remove(separator); hSplitBox.remove(swLinkTreeWidget); separator.hide(); } vpaned.pack1(swPoseListView, true, false); vpaned.pack2(swLinkTreeWidget, true, false); vpaned.show(); } else if(horizontalSplitRadio->get_active()){ vpaned.hide(); if(swPoseListView.get_parent() == &vpaned){ vpaned.remove(swPoseListView); vpaned.remove(swLinkTreeWidget); } hSplitBox.pack_start(swPoseListView); hSplitBox.pack_start(separator, Gtk::PACK_SHRINK); hSplitBox.pack_start(swLinkTreeWidget, Gtk::PACK_SHRINK); separator.show(); hSplitBox.show(); } } PoseSeqView::~PoseSeqView() { delete impl; } PoseSeqViewImpl::~PoseSeqViewImpl() { } void PoseSeqViewImpl::onModeMenuItemToggled(Gtk::RadioMenuItem* menuItem) { if(menuItem->get_active()){ onModeToggled(); } } void PoseSeqViewImpl::onModeToggled() { isLipSyncMode = lipsyncModeRadio->get_active(); actualPronunPoseViewColumn->set_visible(isLipSyncMode); transitionTimeViewColumn->set_visible(!isLipSyncMode); labelViewColumn->set_title(isLipSyncMode ? _("pronunciation / label") : _("label")); if(!isLipSyncMode){ swLinkTreeWidget.show(); } else { swLinkTreeWidget.hide(); } resetPoseListViewModel(); } void PoseSeqViewImpl::onSplitModeMenuItemToggled(Gtk::RadioMenuItem* menuItem) { if(menuItem->get_active()){ updatePlacementOfPoseListViewAndLinkTreeView(); } } void PoseSeqViewImpl::setCurrentPoseSeqItem(PoseSeqItemPtr poseSeqItem) { PoseSeqViewBase::setCurrentPoseSeqItem(poseSeqItem); resetPoseListViewModel(); } void PoseSeqViewImpl::resetPoseListViewModel() { listSelectionChangedConnection.block(); poseListView.unset_model(); poseRefToRowIterMap.clear(); if(seq){ listStore = Gtk::ListStore::create(record); for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){ if((!isLipSyncMode && p->get()) || (isLipSyncMode && p->get())){ Gtk::TreeIter iter = listStore->append(); Gtk::TreeRow row = *iter; row[poseIterColumn] = p; poseRefToRowIterMap[&(*p)] = iter; } } poseListView.set_model(listStore); onListSelectionChanged(); } listSelectionChangedConnection.unblock(); } void PoseSeqViewImpl::nameCellDataFunction(Gtk::CellRenderer* cell, const Gtk::TreeModel::iterator& iter) { const Gtk::TreeRow& row = *iter; PoseSeq::iterator it = row[poseIterColumn]; cell->set_property("text", ustring(it->name())); PronunSymbolPtr pronun = it->get(); if((!isLipSyncMode && !pronun) || (isLipSyncMode && pronun)){ cell->set_property("editable", true); cell->set_property("foreground-gdk", poseListView.get_style()->get_text(Gtk::STATE_NORMAL)); } else { cell->set_property("editable", false); cell->set_property("foreground-gdk", poseListView.get_style()->get_text(Gtk::STATE_INSENSITIVE)); } } void PoseSeqViewImpl::actualPronunPoseCellDataFunction(Gtk::CellRenderer* cell, const Gtk::TreeModel::iterator& iter) { const Gtk::TreeRow& row = *iter; PoseSeq::iterator it = row[poseIterColumn]; PronunSymbolPtr pronun = it->get(); if(pronun){ PoseUnitPtr actualPoseUnit = pronun->actualPoseUnit(); if(actualPoseUnit){ cell->set_property("text", actualPoseUnit->name()); } else { cell->set_property("text", ustring()); } //cell->set_property("foreground-gdk", poseListView.get_style()->get_text(Gtk::STATE_NORMAL)); } else { cell->set_property("text", ustring()); //cell->set_property("foreground-gdk", poseListView.get_style()->get_text(Gtk::STATE_INSENSITIVE)); } } void PoseSeqViewImpl::timeCellDataFunction (Gtk::CellRenderer* cell, const Gtk::TreeModel::iterator& iter) { const Gtk::TreeRow& row = *iter; PoseSeq::iterator it = row[poseIterColumn]; static format f(" % 6.2f "); string text(str(f % it->time())); cell->set_property("text", text); } void PoseSeqViewImpl::transitionTimeCellDataFunction (Gtk::CellRenderer* cell, const Gtk::TreeModel::iterator& iter) { const Gtk::TreeRow& row = *iter; PoseSeq::iterator it = row[poseIterColumn]; string text(str(format(" % 6.2f ") % it->maxTransitionTime())); cell->set_property("text", text); cell->set_property("editable", true); } void PoseSeqViewImpl::onPoseInserted(PoseSeq::iterator it, bool isMoving) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewImpl::onPoseInserted()" << endl; } PoseSeqViewBase::onPoseInserted(it, isMoving); if((!isLipSyncMode && it->get()) || (isLipSyncMode && it->get())){ if(TRACE_FUNCTIONS){ cout << "PoseSeqViewImpl::onPoseInserted(): 2" << endl; } Gtk::TreeIter newTreeIter; PoseSeq::iterator next = it; next++; while(next != seq->end()){ if((!isLipSyncMode && next->get()) || (isLipSyncMode && next->get())){ break; } next++; } if(next != seq->end()){ PoseRefToRowIterMap::iterator p = poseRefToRowIterMap.find(&(*next)); if(p != poseRefToRowIterMap.end()){ Gtk::TreeIter pos = p->second; newTreeIter = listStore->insert(pos); } } else { newTreeIter = listStore->append(); } if(newTreeIter){ Gtk::TreeRow row = *newTreeIter; row[poseIterColumn] = it; poseRefToRowIterMap[&(*it)] = newTreeIter; } } } void PoseSeqViewImpl::onPoseRemoving(PoseSeq::iterator it, bool isMoving) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewImpl::onPoseRemoving()" << endl; } PoseSeqViewBase::onPoseRemoving(it, isMoving); PoseRefToRowIterMap::iterator p = poseRefToRowIterMap.find(&(*it)); if(p != poseRefToRowIterMap.end()){ Gtk::TreeIter treeIter = p->second; poseRefToRowIterMap.erase(p); listStore->erase(treeIter); } } bool PoseSeqViewImpl::getSelectedPoseIters(vector& iters) { iters.clear(); vector selected = listSelection->get_selected_rows(); for(size_t i=0; i < selected.size(); ++i){ const Gtk::TreeRow& row = *listStore->get_iter(selected[i]); iters.push_back(row[poseIterColumn]); } return !iters.empty(); } void PoseSeqViewImpl::onListSelectionChanged() { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewImpl::onListSelectionChanged()" << endl; } selectedPoseIters.clear(); vector selected = listSelection->get_selected_rows(); if(!selected.empty()){ for(size_t i=0; i < selected.size(); ++i){ const Gtk::TreeModel::Row& row = *listStore->get_iter(selected[i]); PoseSeq::iterator p = row[poseIterColumn]; selectedPoseIters.insert(p); currentPoseIter = p; } } updateLinkTreeModel(); onSelectedPosesModified(); if(!selectedPoseIters.empty()){ currentTime = (*selectedPoseIters.begin())->time(); if(timeSyncCheck.get_active() && selectedPoseIters.size() == 1){ timeBar->setTime(currentTime); } } } bool PoseSeqViewImpl::onTimeChanged(double time) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewImpl::onTimeChanged(" << time << ")" << endl; } bool doContinue = false; if(seq && timeSyncCheck.get_active()){ PoseSeq::iterator prevPoseIter = currentPoseIter; currentPoseIter = seq->seek(currentPoseIter, time); if(currentPoseIter == seq->end() || currentPoseIter->time() > time){ if(currentPoseIter == seq->begin()){ currentPoseIter = seq->end(); } else { currentPoseIter--; } } if(toggleSelection(currentPoseIter, false, false)){ if(TRACE_FUNCTIONS){ cout << "PoseSeqViewImpl::actually do pose selection change" << endl; } listSelectionChangedConnection.block(); listSelection->unselect_all(); if(currentPoseIter != seq->end()){ PoseRefToRowIterMap::iterator p = poseRefToRowIterMap.find(&(*currentPoseIter)); if(p != poseRefToRowIterMap.end()){ Gtk::TreeIter treeIter = p->second; listSelection->select(treeIter); poseListView.scroll_to_row(Gtk::TreePath(treeIter)); } doContinue = true; } listSelectionChangedConnection.unblock(); } } currentTime = time; return false; } void PoseSeqViewImpl::onInsertPoseButtonClicked() { if(seq){ PoseSeq::iterator poseIter; if(isLipSyncMode){ poseIter = insertPronunSymbol(); } else if(currentBodyItem){ poseIter = insertPose(); } if(poseIter != seq->end()){ Gtk::TreeIter treeIter = poseRefToRowIterMap[&(*poseIter)]; listSelectionChangedConnection.block(); listSelection->unselect_all(); listSelectionChangedConnection.unblock(); listSelection->select(treeIter); } } } Gtk::TreeIter PoseSeqViewImpl::getListLastIter() { tmpLastIter = Gtk::TreeIter(); // clear listStore->foreach_iter(sigc::mem_fun(*this, &PoseSeqViewImpl::getListLastIterCallback)); return tmpLastIter; } bool PoseSeqViewImpl::getListLastIterCallback(const Gtk::TreeModel::iterator& iter) { tmpLastIter = iter; return false; } void PoseSeqViewImpl::onPoseTimeEdited(const Glib::ustring& path, const Glib::ustring& text) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewImpl::onPoseTimeEdited()" << endl; } if(currentPoseSeqItem){ optional newTime = convertTextToDouble(text); if(newTime){ Gtk::TreeRow row = *listStore->get_iter(path); PoseSeq::iterator poseIter = row[poseIterColumn]; seq->changeTime(poseIter, *newTime); } } } void PoseSeqViewImpl::onPoseNameEdited(const Glib::ustring& path, const Glib::ustring& text) { if(currentPoseSeqItem){ poseSeqConnections.block(); Gtk::TreeIter iter = listStore->get_iter(path); Gtk::TreeRow row = *iter; PoseSeq::iterator poseIter = row[poseIterColumn]; seq->beginPoseModification(poseIter); seq->rename(poseIter, text); seq->endPoseModification(poseIter); poseSeqConnections.unblock(); if(isLipSyncMode){ bool nextFound = false; ++poseIter; while(poseIter != seq->end()){ if(poseIter->get()){ nextFound = true; break; } ++poseIter; } // begin to edit the next symbol if(nextFound){ PoseRefToRowIterMap::iterator p = poseRefToRowIterMap.find(&(*poseIter)); if(p != poseRefToRowIterMap.end()){ Gtk::TreeIter treeIter = p->second; listSelectionChangedConnection.block(); listSelection->unselect_all(); listSelection->select(treeIter); Gtk::TreePath path(treeIter); poseListView.scroll_to_row(path); listSelectionChangedConnection.unblock(); toggleSelection(poseIter, false, true); poseListView.set_cursor(path, *labelViewColumn, true); } } } } } void PoseSeqViewImpl::onPoseTransitionTimeEdited(const Glib::ustring& path, const Glib::ustring& text) { if(currentPoseSeqItem){ optional newMaxTransitionTime = convertTextToDouble(text); if(newMaxTransitionTime){ Gtk::TreeRow row = *listStore->get_iter(path); PoseSeq::iterator poseIter = row[poseIterColumn]; poseSeqConnections.block(); seq->beginPoseModification(poseIter); poseIter->setMaxTransitionTime(*newMaxTransitionTime); seq->endPoseModification(poseIter); doAutomaticInterpolationUpdate(); // This should be done by slots poseSeqConnections.unblock(); } } } bool PoseSeqViewImpl::onPoseListViewKeyPressEvent(GdkEventKey* event) { switch(event->keyval){ case GDK_Escape: // see the comment in onPoseListViewKeyPressEvent() //listSelection->unselect_all(); poseListView.unset_model(); poseListView.set_model(listStore); return true; } return false; } bool PoseSeqViewImpl::onPoseListViewButtonPressEvent(GdkEventButton* event) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewImpl::onPoseListViewButtonPressEvent()" << endl; } bool doDefaultHandler = true; poseListView.grab_focus(); int x, y; Gtk::TreeViewColumn* column = 0; isPressedPathValid = poseListView.get_path_at_pos(event->x, event->y, pressedPath, column, x, y); if(event->type == GDK_BUTTON_PRESS){ if(!isPressedPathValid && (event->button == 1 || event->button == 3)){ // @bug Here we want to unselect all the rows when a user presses a point not on a valid row. // TreeSelection::unselect_all() cannot be used for this purpose because // after unselect_all(), clicking a cell which was selected before doing unselect_all() results // in not selecting the row but immediately entering the edit mode of the cell. // I could not find any solution to avoid this behavior as far as unselect_all() is used. // (For example, setting 'false' to the 'editable' attribute of the cell before calling unselect_all() // and setting 'true' to the attribute again after the call falls into the same result. // This behavior seems a bug of GTK+, so currently realizing unselect_all() should be // done by the code like the followings. poseListView.unset_model(); poseListView.set_model(listStore); //restoreExpansionState(); } if(event->button == 3){ popupMenu.popup(event->button, event->time); if(listSelection->count_selected_rows() > 0){ doDefaultHandler = false; } } } return !doDefaultHandler; } void PoseSeqViewImpl::onCutActivated() { if(seq){ listSelectionChangedConnection.block(); cutSelectedPoses(); listSelectionChangedConnection.unblock(); onListSelectionChanged(); } } void PoseSeqViewImpl::onCopyActivated() { if(seq){ copySelectedPoses(); } } void PoseSeqViewImpl::onPasteActivated() { if(seq){ pasteCopiedPoses(timeBar->time()); } } MoveOperationDialog::MoveOperationDialog() : Gtk::Dialog(_("Move Selected Poses"), true), timeSpinLabel(_("New time of the first pose: ")) { Gtk::VBox* vbox = get_vbox(); Gtk::HBox* hbox = Gtk::manage(new Gtk::HBox()); hbox->pack_start(timeSpinLabel, Gtk::PACK_SHRINK); timeSpin.set_digits(2); timeSpin.set_range(-999.99, 999.99); timeSpin.set_increments(0.01, 0.1); hbox->pack_start(timeSpin, Gtk::PACK_SHRINK); vbox->pack_start(*hbox, Gtk::PACK_SHRINK); add_button(_("Ok"), Gtk::RESPONSE_OK); add_button(_("Cancel"), Gtk::RESPONSE_CANCEL); show_all_children(); } void PoseSeqViewImpl::onMoveActivated() { if(seq){ vector selectedPoseIters; if(getSelectedPoseIters(selectedPoseIters)){ const double orgTime = selectedPoseIters.front()->time(); moveOperationDialog.timeSpin.set_value(orgTime); if(moveOperationDialog.run() == Gtk::RESPONSE_OK){ const double newTime = moveOperationDialog.timeSpin.get_value(); const double timeDiff = newTime - orgTime; if(timeDiff != 0.0){ for(size_t i=0; i < selectedPoseIters.size(); ++i){ PoseSeq::iterator& iter = selectedPoseIters[i]; seq->changeTime(iter, iter->time() + timeDiff); } } } moveOperationDialog.hide(); } } } bool PoseSeqView::storeState(Archive& archive) { return impl->storeState(archive); } bool PoseSeqViewImpl::storeState(Archive& archive) { if(PoseSeqViewBase::storeState(archive)){ archive.write("mode", (isLipSyncMode ? "lipsync" : "normal")); archive.write("splitMode", (verticalSplitRadio->get_active() ? "vertical" : "horizontal")); if(verticalSplitRadio->get_active()){ archive.write("splitPosition", vpaned.get_position()); } return true; } return false; } bool PoseSeqView::restoreState(const Archive& archive) { return impl->restoreState(archive); } bool PoseSeqViewImpl::restoreState(const Archive& archive) { if(PoseSeqViewBase::restoreState(archive)){ string mode = archive.get("mode", (isLipSyncMode ? "lipsync" : "normal")); if(mode == "normal"){ normalModeRadio->set_active(true); } else if(mode == "lipsync"){ lipsyncModeRadio->set_active(true); } string splitMode = archive.get("splitMode", (verticalSplitRadio->get_active() ? "vertical" : "horizontal")); if(splitMode == "vertical"){ verticalSplitRadio->set_active(true); int position; if(archive.read("splitPosition", position)){ vpaned.set_position(position); } } else if(splitMode == "horizontal"){ horizontalSplitRadio->set_active(true); } return true; } return false; } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqView.h000066400000000000000000000011321207742442300224730ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_CHOREOGRAPHY_BODY_POSE_SEQ_VIEW_H_INCLUDED #define CNOID_CHOREOGRAPHY_BODY_POSE_SEQ_VIEW_H_INCLUDED #include namespace cnoid { class Archive; class ExtensionManager; class PoseSeqViewImpl; class PoseSeqView : public cnoid::View { public: PoseSeqView(ExtensionManager& ext); ~PoseSeqView(); private: PoseSeqViewImpl* impl; virtual bool storeState(Archive& archive); virtual bool restoreState(const Archive& archive); }; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqViewBase.cpp000066400000000000000000001610341207742442300236310ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "PoseSeqViewBase.h" #include "BodyMotionGenerationBar.h" #include "PronunSymbol.h" #include "PoseFilters.h" #include "BodyMotionGenerationBar.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include "gettext.h" using namespace std; using namespace boost; using namespace cnoid; namespace { const bool TRACE_FUNCTIONS = false; inline double degree(double rad) { return (180.0 * rad / 3.14159265358979); } inline double radian(double deg) { return (3.14159265358979 * deg / 180.0); } class ColumnCheckBox : public CheckBox { public: ColumnCheckBox(function slotOnClicked) : slotOnClicked(slotOnClicked) { } virtual void nextCheckState(){ slotOnClicked(checkState()); }; function slotOnClicked; }; class LinkTreeWidgetEx : public LinkTreeWidget { public: LinkTreeWidgetEx(QWidget* parent) : LinkTreeWidget(parent) { header()->setResizeMode(nameColumn(), QHeaderView::ResizeToContents); } virtual QSize sizeHint() const { QSize size = QTreeWidget::sizeHint(); int width = header()->length(); size.setWidth(width); return size; } }; } namespace cnoid { class LinkPositionAdjustmentDialog : public Dialog { public: RadioButton absoluteRadio; RadioButton relativeRadio; CheckBox targetAxisCheck[3]; DoubleSpinBox positionSpin[3]; LinkPositionAdjustmentDialog(View* parentView) : Dialog(parentView) { setWindowTitle(_("Link Position Adjustment")); QVBoxLayout* vbox = new QVBoxLayout(); QHBoxLayout* hbox = new QHBoxLayout(); vbox->addLayout(hbox); absoluteRadio.setText(_("Absolute")); hbox->addWidget(&absoluteRadio); relativeRadio.setText(_("Relative")); relativeRadio.setChecked(true); hbox->addWidget(&relativeRadio); hbox = new QHBoxLayout(); vbox->addLayout(hbox); const char* axisLabel[] = { "X", "Y", "Z" }; for(int i=0; i < 3; ++i){ targetAxisCheck[i].setText(axisLabel[i]); hbox->addWidget(&targetAxisCheck[i]); positionSpin[i].setDecimals(3); positionSpin[i].setRange(-99.999, 99.999); positionSpin[i].setSingleStep(0.001); positionSpin[i].setValue(0.0); hbox->addWidget(&positionSpin[i]); } QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok); connect(buttonBox, SIGNAL(accepted()), this, SLOT(accept())); vbox->addWidget(buttonBox); setLayout(vbox); } bool storeState(Archive& archive) { return true; } bool restoreState(const Archive& archive) { return true; } }; class YawOrientationRotationDialog : public Dialog { public: DoubleSpinBox angleSpin; DoubleSpinBox centerPosSpins[2]; YawOrientationRotationDialog(View* parentView) : Dialog(parentView) { setWindowTitle(_("Yaw Orientation Rotation")); QVBoxLayout* vbox = new QVBoxLayout(); QHBoxLayout* hbox = new QHBoxLayout(); vbox->addLayout(hbox); hbox->addWidget(new QLabel(_("Center:"))); hbox->addSpacing(8); const char* axisLabel[] = { "X", "Y" }; for(int i=0; i < 2; ++i){ hbox->addWidget(new QLabel(axisLabel[i])); centerPosSpins[i].setDecimals(3); centerPosSpins[i].setRange(-99.999, 99.999); centerPosSpins[i].setSingleStep(0.001); hbox->addWidget(¢erPosSpins[i]); } hbox = new QHBoxLayout(); vbox->addLayout(hbox); hbox->addWidget(new QLabel(_("Angle"))); angleSpin.setDecimals(1); angleSpin.setRange(0.1, 90.0); angleSpin.setSingleStep(0.1); hbox->addWidget(&angleSpin); hbox->addWidget(new QLabel(_("[deg]"))); QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok); connect(buttonBox, SIGNAL(accepted()), this, SLOT(accept())); vbox->addWidget(buttonBox); setLayout(vbox); } bool storeState(Archive& archive) { return true; } bool restoreState(const Archive& archive) { return true; } }; class PoseSelectionDialog : public Dialog { public: DoubleSpinBox startTimeSpin; DoubleSpinBox endTimeSpin; RadioButton allPartRadio; RadioButton selectedPartRadio; RadioButton justSelectedPartRadio; PoseSelectionDialog(View* parentView) : Dialog(parentView) { setWindowTitle(_("Select Specified Key Poses")); QVBoxLayout* vbox = new QVBoxLayout(); QHBoxLayout* hbox = new QHBoxLayout(); vbox->addLayout(hbox); hbox->addWidget(new QLabel(_("Start"))); startTimeSpin.setDecimals(2); startTimeSpin.setRange(0.00, 999.99); startTimeSpin.setSingleStep(0.01); hbox->addWidget(&startTimeSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox->addWidget(new QLabel(_("End"))); endTimeSpin.setDecimals(2); endTimeSpin.setRange(0.00, 999.99); endTimeSpin.setSingleStep(0.01); hbox->addWidget(&endTimeSpin); hbox->addWidget(new QLabel(_("[s]"))); hbox = new QHBoxLayout(); vbox->addLayout(hbox); allPartRadio.setText(_("all parts")); hbox->addWidget(&allPartRadio); selectedPartRadio.setText(_("having selected parts")); selectedPartRadio.setChecked(true); hbox->addWidget(&selectedPartRadio); justSelectedPartRadio.setText(_("just selected parts")); hbox->addWidget(&justSelectedPartRadio); QDialogButtonBox* buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok); connect(buttonBox, SIGNAL(accepted()), this, SLOT(accept())); vbox->addWidget(buttonBox); setLayout(vbox); } bool storeState(Archive& archive) { return true; } bool restoreState(const Archive& archive) { return true; } }; } PoseSeqViewBase::PoseSeqViewBase(View* view) : view(view), os(MessageView::mainInstance()->cout()), textForEmptyName("----------"), menuManager(&popupMenu) { view->sigActivated().connect(bind(&PoseSeqViewBase::onViewActivated, this)); view->sigDeactivated().connect(bind(&PoseSeqViewBase::onViewDeactivated, this)); currentTime = 0.0; timeBar = TimeBar::instance(); BodyMotionGenerationBar* generationBar = BodyMotionGenerationBar::instance(); timeScale = generationBar->timeScaleRatio(); generationBar->sigInterpolationParametersChanged().connect( bind(&PoseSeqViewBase::onInterpolationParametersChanged, this)); setupOperationParts(); setupLinkTreeWidget(); ItemTreeView::mainInstance()->sigSelectionChanged().connect( bind(&PoseSeqViewBase::onItemSelectionChanged, this, _1)); isSelectedPoseMoving = false; copiedPoses = new PoseSeq(); poseSelectionDialog = new PoseSelectionDialog(view); poseSelectionDialog->sigAccepted().connect( bind(&PoseSeqViewBase::onPoseSelectionDialogAccepted, this)); linkPositionAdjustmentDialog = new LinkPositionAdjustmentDialog(view); linkPositionAdjustmentDialog->sigAccepted().connect( bind(&PoseSeqViewBase::onLinkPositionAdjustmentDialogAccepted, this)); yawOrientationRotationDialog = new YawOrientationRotationDialog(view); yawOrientationRotationDialog->sigAccepted().connect( bind(&PoseSeqViewBase::onYawOrientationRotationDialogAccepted, this)); menuManager.addItem(_("Select all poses after current position"))->sigTriggered().connect (bind(&PoseSeqViewBase::selectAllPosesAfterCurrentPosition, this)); menuManager.addItem(_("Select all poses before current position"))->sigTriggered().connect (bind(&PoseSeqViewBase::selectAllPosesBeforeCurrentPosition, this)); menuManager.addItem(_("Adjust step positions"))->sigTriggered().connect (bind(&PoseSeqViewBase::onAdjustStepPositionsActivated, this)); menuManager.addItem(_("Count selected key poses"))->sigTriggered().connect( bind(&PoseSeqViewBase::countSelectedKeyPoses, this)); } PoseSeqViewBase::~PoseSeqViewBase() { poseSeqConnections.disconnect(); connectionOfBodyKinematicStateEdited.disconnect(); } void PoseSeqViewBase::onViewActivated() { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::onViewActivated()" << endl; } if(timeSyncCheck.isChecked()){ if(!connectionOfTimeChanged.connected()){ connectionOfTimeChanged = timeBar->sigTimeChanged().connect( bind(&PoseSeqViewBase::onTimeChanged, this, _1)); } onTimeChanged(timeBar->time()); } } void PoseSeqViewBase::onViewDeactivated() { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::onViewDeactivated()" << endl; } connectionOfTimeChanged.disconnect(); } void PoseSeqViewBase::onTimeSyncCheckToggled() { if(timeSyncCheck.isChecked()){ if(!connectionOfTimeChanged.connected()){ connectionOfTimeChanged = timeBar->sigTimeChanged().connect( bind(&PoseSeqViewBase::onTimeChanged, this, _1)); } } else { connectionOfTimeChanged.disconnect(); } } void PoseSeqViewBase::setupOperationParts() { currentItemLabel.setText(textForEmptyName); currentItemLabel.setAlignment(Qt::AlignCenter); insertPoseButton.setText(_(" Insert ")); insertPoseButton.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); insertPoseButton.setToolTip(_("Insert a new pose at the current time position")); insertPoseButton.sigClicked().connect( bind(&PoseSeqViewBase::onInsertPoseButtonClicked, this)); transitionTimeSpin.setToolTip(_("Transition time of a newly inserted pose")); transitionTimeSpin.setAlignment(Qt::AlignCenter); transitionTimeSpin.setDecimals(3); transitionTimeSpin.setRange(0.0, 9.999); transitionTimeSpin.setSingleStep(0.005); transitionTimeSpin.sigEditingFinished().connect( bind(&PoseSeqViewBase::onInsertPoseButtonClicked, this)); updateButton.setText(_("Update")); updateButton.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); updateButton.setToolTip(_("Update the selected pose with the current robot state")); updateButton.sigClicked().connect( bind(&PoseSeqViewBase::onUpdateButtonClicked, this)); updateAllToggle.setText(_("All")); updateAllToggle.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); updateAllToggle.setToolTip(_("The update button updates all the element of the selected pose.")); updateAllToggle.setChecked(true); autoUpdateModeCheck.setText(_("Auto")); autoUpdateModeCheck.setToolTip(_("The selected pose is automatically updated when the robot state changes.")); autoUpdateModeCheck.setChecked(false); deleteButton.setText(_("Delete")); deleteButton.setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); deleteButton.sigClicked().connect( bind(&PoseSeqViewBase::onDeleteButtonClicked, this)); timeSyncCheck.setText(_("Time sync")); timeSyncCheck.setChecked(true); timeSyncCheck.sigToggled().connect( bind(&PoseSeqViewBase::onTimeSyncCheckToggled, this)); } void PoseSeqViewBase::setupLinkTreeWidget() { linkTreeWidget = new LinkTreeWidgetEx(view); //linkTreeWidget->setStyleSheet("QTreeView::item { border-right: 1px solid black }"); QHeaderView* header = linkTreeWidget->header(); header->hideSection(linkTreeWidget->jointIdColumn()); poseForDefaultStateSetting = new Pose(); baseLinkColumn = linkTreeWidget->addColumn("BL"); header->setResizeMode(baseLinkColumn, QHeaderView::ResizeToContents); linkTreeWidget->moveVisualColumnIndex(baseLinkColumn, 0); baseLinkRadioGroup = 0; validPartColumn = linkTreeWidget->addColumn("ON"); header->setResizeMode(validPartColumn, QHeaderView::ResizeToContents); stationaryPointColumn = linkTreeWidget->addColumn("SP"); header->setResizeMode(stationaryPointColumn, QHeaderView::ResizeToContents); ikPartColumn = linkTreeWidget->addColumn("IK"); header->setResizeMode(ikPartColumn, QHeaderView::ResizeToContents); zmpRow = new LinkTreeItem("ZMP"); linkTreeWidget->addCustomRow(zmpRow); linkTreeWidget->sigUpdateRequest().connect( bind(&PoseSeqViewBase::onLinkTreeUpdateRequest, this, _1)); linkTreeWidget->setFrameShape(QFrame::NoFrame); linkTreeWidget->setDefaultExpansionLevel(1); linkTreeWidget->enableCache(true); linkTreeWidget->setListingMode(LinkTreeWidget::PART_TREE); linkTreeWidget->fixListingMode(true); MenuManager& mm = linkTreeWidget->popupMenuManager(); mm.addItem(_("Select key poses having the selected links"))->sigTriggered().connect( bind(&PoseSeqViewBase::selectPosesHavingSelectedLinks, this)); mm.addItem(_("Select key poses just having the selected links"))->sigTriggered().connect( bind(&PoseSeqViewBase::selectPosesJustHavingSelectedLinks, this)); mm.addItem(_("Remove the selected parts from the selected poses"))->sigTriggered().connect( bind(&PoseSeqViewBase::removeSelectedPartsFromKeyPoses, this)); } bool PoseSeqViewBase::isChecked(LinkTreeItem* item, int column) { QAbstractButton* check = dynamic_cast(linkTreeWidget->alignedItemWidget(item, column)); return check ? check->isChecked() : Qt::Unchecked; } void PoseSeqViewBase::setChecked(LinkTreeItem* item, int column, bool checked) { QAbstractButton* check = dynamic_cast(linkTreeWidget->alignedItemWidget(item, column)); if(check){ check->setChecked(checked); } } void PoseSeqViewBase::setCheckState(LinkTreeItem* item, int column, Qt::CheckState state) { QCheckBox* check = dynamic_cast(linkTreeWidget->alignedItemWidget(item, column)); if(check){ check->setCheckState(state); } } void PoseSeqViewBase::onLinkTreeUpdateRequest(bool isInitialCreation) { if(isInitialCreation){ initializeLinkTree(); } updateLinkTreeModel(); } void PoseSeqViewBase::initializeLinkTree() { poseForDefaultStateSetting->clear(); if(baseLinkRadioGroup){ delete baseLinkRadioGroup; } baseLinkRadioGroup = new ButtonGroup(linkTreeWidget); baseLinkRadioGroup->sigButtonClicked().connect( bind(&PoseSeqViewBase::onBaseLinkRadioClicked, this)); initializeLinkTreeIkLinkColumn(); Link* rootLink = body->rootLink(); poseForDefaultStateSetting->setBaseLink(rootLink->index, rootLink->p, rootLink->R); initializeLinkTreeTraverse(linkTreeWidget->invisibleRootItem()); } void PoseSeqViewBase::initializeLinkTreeIkLinkColumn() { const YamlMapping& info = *body->info(); possibleIkLinkFlag.resize(body->numLinks()); possibleIkLinkFlag.reset(); const YamlSequence& possibleIkLinks = *info.findSequence("possibleIkInterpolationLinks"); if(possibleIkLinks.isValid()){ for(int i=0; i < possibleIkLinks.size(); ++i){ Link* link = body->link(possibleIkLinks[i]); if(link){ possibleIkLinkFlag[link->index] = true; LinkTreeItem* item = linkTreeWidget->itemOfLink(link->index); if(item){ ColumnCheckBox* checkBox = new ColumnCheckBox( bind(&PoseSeqViewBase::onIkPartCheckClicked, this, item, _1)); linkTreeWidget->setAlignedItemWidget(item, ikPartColumn, checkBox); } } } } const YamlSequence& defaultIkLinks = *info.findSequence("defaultIkInterpolationLinks"); if(defaultIkLinks.isValid()){ for(int i=0; i < defaultIkLinks.size(); ++i){ Link* link = body->link(defaultIkLinks[i]); if(link){ poseForDefaultStateSetting->addIkLink(link->index); } } } } void PoseSeqViewBase::initializeLinkTreeTraverse(QTreeWidgetItem* parentItem) { int n = parentItem->childCount(); for(int i=0; i < n; ++i){ LinkTreeItem* item = dynamic_cast(parentItem->child(i)); if(!item){ continue; } Link* link = item->link(); if(!link || link->parent){ ColumnCheckBox* checkBox = new ColumnCheckBox( bind(&PoseSeqViewBase::onValidPartCheckClicked, this, item, _1)); if(link && link->jointId >= 0){ poseForDefaultStateSetting->setJointPosition(link->jointId, 0.0); } else if(item->isLinkGroup()){ checkBox->setTristate(true); } linkTreeWidget->setAlignedItemWidget(item, validPartColumn, checkBox); } if(link){ RadioButton* radioButton = new RadioButton(); baseLinkRadioGroup->addButton(radioButton, link->index); linkTreeWidget->setAlignedItemWidget(item, baseLinkColumn, radioButton); } ColumnCheckBox* spCheck = new ColumnCheckBox( bind(&PoseSeqViewBase::onStationaryPointCheckClicked, this, item, _1)); linkTreeWidget->setAlignedItemWidget(item, stationaryPointColumn, spCheck); initializeLinkTreeTraverse(item); } } void PoseSeqViewBase::togglePoseAttribute(boost::function toggleFunction) { if(selectedPoseIters.empty()){ if(toggleFunction(poseForDefaultStateSetting)){ updateLinkTreeModel(); } } else { currentPoseSeqItem->beginEditing(); bool modified = false; for(PoseIterSet::iterator p = selectedPoseIters.begin(); p != selectedPoseIters.end(); ++p){ PosePtr pose = (*p)->get(); if(pose){ seq->beginPoseModification(*p); modified = toggleFunction(pose); if(modified){ seq->endPoseModification(*p); } } } currentPoseSeqItem->endEditing(modified); //! \todo This should be processed by PoseSeq slots if(modified){ doAutomaticInterpolationUpdate(); } } } void PoseSeqViewBase::onBaseLinkRadioClicked() { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::onBaseLinkRadioClicked()" << endl; } int linkIndex = baseLinkRadioGroup->checkedId(); Link* link = (linkIndex >= 0) ? body->link(linkIndex) : 0; togglePoseAttribute(bind(&PoseSeqViewBase::setBaseLink, this, _1, link)); } bool PoseSeqViewBase::setBaseLink(PosePtr& pose, Link* link) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::toggleBaseLink()" << endl; } bool modified = false; if(link){ if(pose->baseLinkIndex() != link->index){ pose->setBaseLink(link->index, link->p, link->R); modified = true; } } else { if(pose->baseLinkInfo()){ pose->invalidateBaseLink(); modified = true; } } return modified; } void PoseSeqViewBase::onValidPartCheckClicked(LinkTreeItem* item, Qt::CheckState checkState) { bool on = ((checkState == Qt::Unchecked) || (checkState == Qt::PartiallyChecked)); if(item == zmpRow){ togglePoseAttribute(bind(&PoseSeqViewBase::toggleZmp, this, _1, on)); } else { Link* link = item->link(); if(link){ bool isIkPartChecked = isChecked(item, ikPartColumn); togglePoseAttribute(bind(&PoseSeqViewBase::toggleLink, this, _1, item, link, on, isIkPartChecked)); } else { togglePoseAttribute(bind(&PoseSeqViewBase::togglePart, this, _1, item, on)); } } } bool PoseSeqViewBase::toggleZmp(PosePtr& pose, bool on) { bool modified = false; if(on){ const Vector3& zmp = currentBodyItem->zmp(); if(!pose->isZmpValid() || zmp != pose->zmp()){ pose->setZmp(currentBodyItem->zmp()); modified = true; } } else { if(pose->isZmpValid()){ pose->invalidateZmp(); modified = true; } } return modified; } bool PoseSeqViewBase::toggleLink(PosePtr& pose, LinkTreeItem* item, Link* link, bool partOn, bool ikOn) { bool modified = false; int jId = link->jointId; if(partOn){ if(jId >= 0){ bool isSpChecked = isChecked(item, stationaryPointColumn); if(!pose->isJointValid(jId) || pose->jointPosition(jId) != link->q || pose->isJointStationaryPoint(jId) != isSpChecked){ pose->setJointPosition(jId, link->q); pose->setJointStationaryPoint(jId, isSpChecked); modified = true; } } if(possibleIkLinkFlag[link->index]){ Pose::LinkInfo* info = pose->ikLinkInfo(link->index); if(!info){ info = pose->addIkLink(link->index); modified = true; } if(setCurrentLinkStateToIkLink(link, info)){ modified = true; } bool isSlave = !ikOn; if(info->isSlave() != isSlave){ info->setSlave(isSlave); modified = true; } } } else { if(pose->isJointValid(jId)){ pose->invalidateJoint(jId); modified = true; } if(pose->removeIkLink(link->index)){ modified = true; } } return modified; } bool PoseSeqViewBase::togglePart(PosePtr& pose, LinkTreeItem* item, bool on) { bool modified = false; Link* link = item->link(); if(link){ bool ikOn = false; if(possibleIkLinkFlag[link->index]){ if(isChecked(item, validPartColumn)){ ikOn = isChecked(item, ikPartColumn); } else { ikOn = on; } } modified = toggleLink(pose, item, link, on, ikOn); } int n = item->childCount(); for(int i=0; i < n; ++i){ LinkTreeItem* childItem = dynamic_cast(item->child(i)); if(childItem){ modified |= togglePart(pose, childItem, on); } } return modified; } void PoseSeqViewBase::onStationaryPointCheckClicked(LinkTreeItem* item, Qt::CheckState checkState) { bool on = (checkState == Qt::Unchecked); if(item == zmpRow){ togglePoseAttribute(bind(&PoseSeqViewBase::toggleZmpStationaryPoint, this, _1, on)); } else { Link* link = item->link(); if(link){ togglePoseAttribute(bind(&PoseSeqViewBase::toggleStationaryPoint, this, _1, link, on)); } else { if(checkState == Qt::PartiallyChecked){ on = true; } togglePoseAttribute(bind(&PoseSeqViewBase::togglePartStationaryPoints, this, _1, item, on)); } } } bool PoseSeqViewBase::toggleZmpStationaryPoint(PosePtr& pose, bool on) { bool modified = false; if(on){ if(!pose->isZmpStationaryPoint()){ pose->setZmpStationaryPoint(on); modified = true; } } else { if(pose->isZmpStationaryPoint()){ pose->setZmpStationaryPoint(on); modified = true; } } return modified; } bool PoseSeqViewBase::toggleStationaryPoint(PosePtr& pose, Link* link, bool on) { bool modified = false; int id = link->jointId; if(pose->isJointValid(id)){ pose->setJointStationaryPoint(id, on); modified = true; } Pose::LinkInfo* info = pose->ikLinkInfo(link->index); if(info){ info->setStationaryPoint(on); modified = true; } return modified; } bool PoseSeqViewBase::togglePartStationaryPoints(PosePtr& pose, LinkTreeItem* item, bool on) { bool modified = false; Link* link = item->link(); if(link){ modified = toggleStationaryPoint(pose, link, on); } int n = item->childCount(); for(int i=0; i < n; ++i){ LinkTreeItem* childItem = dynamic_cast(item->child(i)); if(childItem){ modified |= togglePartStationaryPoints(pose, childItem, on); } } return modified; } void PoseSeqViewBase::onIkPartCheckClicked(LinkTreeItem* item, Qt::CheckState checkState) { Link* link = item->link(); if(link){ bool ikOn = (checkState == Qt::Unchecked); bool partOn = ikOn | isChecked(item, validPartColumn); togglePoseAttribute(bind(&PoseSeqViewBase::toggleLink, this, _1, item, link, partOn, ikOn)); } } void PoseSeqViewBase::onInterpolationParametersChanged() { double newTimeScale = BodyMotionGenerationBar::instance()->timeScaleRatio(); if(newTimeScale != timeScale){ timeScale = newTimeScale; onTimeScaleChanged(); } } void PoseSeqViewBase::onTimeScaleChanged() { onSelectedPosesModified(); } void PoseSeqViewBase::onItemSelectionChanged(const ItemList& selectedItems) { PoseSeqItemPtr item = selectedItems.toSingle(); if(item){ setCurrentPoseSeqItem(item); } } void PoseSeqViewBase::setCurrentPoseSeqItem(PoseSeqItemPtr poseSeqItem) { if(poseSeqItem == currentPoseSeqItem){ return; } poseSeqConnections.disconnect(); currentPoseSeqItem = poseSeqItem; setCurrentItemName(poseSeqItem); connectionOfBodyKinematicStateEdited.disconnect(); seq = 0; currentBodyItem = 0; body = 0; selectedPoseIters.clear(); if(poseSeqItem){ poseSeqConnections.add( poseSeqItem->sigNameChanged().connect( bind(&PoseSeqViewBase::setCurrentItemName, this, poseSeqItem))); seq = currentPoseSeqItem->poseSeq(); currentPoseIter = seq->end(); currentBodyItem = poseSeqItem->findOwnerItem(); if(currentBodyItem){ body = currentBodyItem->body(); } linkTreeWidget->setBodyItem(currentBodyItem); if(currentBodyItem){ connectionOfBodyKinematicStateEdited = currentBodyItem->sigKinematicStateEdited().connect( bind(&PoseSeqViewBase::onBodyKinematicStateEdited, this)); } poseSeqConnections.add( seq->connectSignalSet( bind(&PoseSeqViewBase::onPoseInserted, this, _1, _2), bind(&PoseSeqViewBase::onPoseRemoving, this, _1, _2), bind(&PoseSeqViewBase::onPoseModified, this, _1))); poseSeqConnections.add( poseSeqItem->sigDetachedFromRoot().connect( bind(&PoseSeqViewBase::setCurrentPoseSeqItem, this, PoseSeqItemPtr()))); } } PoseSeqViewBase::PoseIterSet::iterator PoseSeqViewBase::findPoseIterInSelected(PoseSeq::iterator poseIter) { pair range = selectedPoseIters.equal_range(poseIter); for(PoseIterSet::iterator p = range.first; p != range.second; ++p){ if((*p) == poseIter){ return p; } } return selectedPoseIters.end(); } bool PoseSeqViewBase::toggleSelection(PoseSeq::iterator poseIter, bool adding, bool changeTime) { if(!(selectedPoseIters.size() == 1 && *selectedPoseIters.begin() == poseIter)){ // Skip same single selection if(poseIter == seq->end()){ if(selectedPoseIters.empty()){ return false; } selectedPoseIters.clear(); } else { PoseIterSet::iterator p = findPoseIterInSelected(poseIter); if(p == selectedPoseIters.end()){ if(!adding){ selectedPoseIters.clear(); } selectedPoseIters.insert(poseIter); } else { if(adding){ selectedPoseIters.erase(p); } } } updateLinkTreeModel(); onSelectedPosesModified(); } if(changeTime && (poseIter != seq->end())){ double time = timeScale * poseIter->time(); if(timeSyncCheck.isChecked()){ timeBar->setTime(time); } else { onTimeChanged(time); } } return true; } void PoseSeqViewBase::selectAllPoses() { selectedPoseIters.clear(); for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){ selectedPoseIters.insert(p); } updateLinkTreeModel(); onSelectedPosesModified(); } void PoseSeqViewBase::selectAllPosesAfterCurrentPosition() { selectedPoseIters.clear(); PoseSeq::iterator p = seq->seek(seq->begin(), currentTime); while(p != seq->end()){ selectedPoseIters.insert(p++); } updateLinkTreeModel(); onSelectedPosesModified(); } void PoseSeqViewBase::selectAllPosesBeforeCurrentPosition() { selectedPoseIters.clear(); if(!seq->empty()){ PoseSeq::iterator p = seq->seek(seq->begin(), currentTime); if(p != seq->end() && (p->time() == currentTime)){ ++p; } do { selectedPoseIters.insert(--p); } while(p != seq->begin()); } updateLinkTreeModel(); onSelectedPosesModified(); } void PoseSeqViewBase::selectPosesHavingSelectedLinks() { if(!body || !seq){ return; } const vector selectedLinkIndices = linkTreeWidget->getSelectedLinkIndices(); selectedPoseIters.clear(); for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){ PosePtr pose = p->get(); if(pose){ bool match = true; for(size_t i=0; i < selectedLinkIndices.size(); ++i){ int linkIndex = selectedLinkIndices[i]; if(!pose->isJointValid(body->link(linkIndex)->jointId)){ if(!pose->ikLinkInfo(linkIndex)){ match = false; break; } } } if(match){ selectedPoseIters.insert(p); } } } updateLinkTreeModel(); onSelectedPosesModified(); } void PoseSeqViewBase::selectPosesJustHavingSelectedLinks() { if(!body || !seq){ return; } const boost::dynamic_bitset<>& linkSelection = linkTreeWidget->getLinkSelection(); selectedPoseIters.clear(); for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){ PosePtr pose = p->get(); if(pose){ bool match = true; for(size_t linkIndex=0; linkIndex < linkSelection.size(); ++linkIndex){ bool hasLink = pose->isJointValid(body->link(linkIndex)->jointId) || pose->ikLinkInfo(linkIndex); if((linkSelection[linkIndex] && !hasLink) || (!linkSelection[linkIndex] && hasLink)){ match = false; break; } } if(match){ selectedPoseIters.insert(p); } } } updateLinkTreeModel(); onSelectedPosesModified(); } void PoseSeqViewBase::removeSelectedPartsFromKeyPoses() { if(!body || !seq || selectedPoseIters.empty()){ return; } const std::vector& selected = linkTreeWidget->getSelectedLinkIndices(); bool doRemoveZmp = zmpRow->isSelected(); if(selected.empty() && !doRemoveZmp){ return; } PoseIterSet orgSelected(selectedPoseIters); currentPoseSeqItem->beginEditing(); bool removed = false; for(PoseIterSet::iterator p = orgSelected.begin(); p != orgSelected.end(); ++p){ PosePtr pose = (*p)->get(); if(pose){ seq->beginPoseModification(*p); bool modified = false; for(size_t i=0; i < selected.size(); ++i){ int linkIndex = selected[i]; int jointId = body->link(linkIndex)->jointId; if(jointId >= 0){ modified |= pose->invalidateJoint(jointId); } modified |= pose->removeIkLink(linkIndex); } if(doRemoveZmp){ modified |= pose->invalidateZmp(); } if(pose->empty()){ seq->erase(*p); } else if(modified){ seq->endPoseModification(*p); } removed |= modified; } } if(currentPoseSeqItem->endEditing(removed)){ doAutomaticInterpolationUpdate(); } } bool PoseSeqViewBase::deleteSelectedPoses() { if(!selectedPoseIters.empty()){ PoseIterSet orgSelected(selectedPoseIters); currentPoseSeqItem->beginEditing(); for(PoseIterSet::iterator p = orgSelected.begin(); p != orgSelected.end(); ++p){ seq->erase(*p); } currentPoseSeqItem->endEditing(); doAutomaticInterpolationUpdate(); return true; } return false; } bool PoseSeqViewBase::cutSelectedPoses() { if(copySelectedPoses()){ return deleteSelectedPoses(); } return false; } bool PoseSeqViewBase::copySelectedPoses() { if(!selectedPoseIters.empty()){ copiedPoses = new PoseSeq(); PoseSeq::iterator destIter = copiedPoses->begin(); double offset = - (*selectedPoseIters.begin())->time(); for(PoseIterSet::iterator p = selectedPoseIters.begin(); p != selectedPoseIters.end(); ++p){ PoseSeq::iterator srcIter = *p; destIter = copiedPoses->copyElement(destIter, srcIter, offset); } return true; } return false; } bool PoseSeqViewBase::pasteCopiedPoses(double timeToPaste) { if(!copiedPoses->empty()){ currentPoseSeqItem->beginEditing(); PoseSeq::iterator dest = seq->seek(currentPoseIter, timeToPaste, true); for(PoseSeq::iterator p = copiedPoses->begin(); p != copiedPoses->end(); ++p){ dest = seq->copyElement(dest, p, timeToPaste); } currentPoseIter = dest; currentPoseSeqItem->endEditing(); doAutomaticInterpolationUpdate(); return true; } return false; } /** @ret true if actually modified */ bool PoseSeqViewBase::moveSelectedPoses(double time0) { bool modified = false; if(!selectedPoseIters.empty()){ time0 = std::max(0.0, time0); double diff = time0 - (*selectedPoseIters.begin())->time(); if(diff != 0.0){ // Copy is needed because selectedPoseIters may change during the following loops PoseIterSet tmpSelectedPoseIters(selectedPoseIters); if(diff > 0.0){ for(PoseIterSet::reverse_iterator p = tmpSelectedPoseIters.rbegin(); p != tmpSelectedPoseIters.rend(); ++p){ seq->changeTime(*p, (*p)->time() + diff); } } else { for(PoseIterSet::iterator p = tmpSelectedPoseIters.begin(); p != tmpSelectedPoseIters.end(); ++p){ seq->changeTime(*p, (*p)->time() + diff); } } modified = true; } } return modified; } bool PoseSeqViewBase::modifyTransitionTimeOfSelectedPoses(double ttime) { bool modified = false; if(!selectedPoseIters.empty()){ for(PoseIterSet::iterator p = selectedPoseIters.begin(); p != selectedPoseIters.end(); ++p){ seq->beginPoseModification(*p); (*p)->setMaxTransitionTime(ttime); seq->endPoseModification(*p); } modified = true; } return modified; } void PoseSeqViewBase::popupContextMenu(QMouseEvent* event) { popupMenu.popup(event->globalPos()); } void PoseSeqViewBase::onSelectSpecifiedKeyPosesActivated() { poseSelectionDialog->show(); } void PoseSeqViewBase::onPoseSelectionDialogAccepted() { if(!body || !seq){ return; } selectedPoseIters.clear(); const vector selectedLinkIndices = linkTreeWidget->getSelectedLinkIndices(); const double t0 = poseSelectionDialog->startTimeSpin.value(); const double t1 = poseSelectionDialog->endTimeSpin.value(); PoseSeq::iterator p = seq->seek(seq->begin(), t0); while(p != seq->end()){ if(p->time() > t1){ break; } if(poseSelectionDialog->selectedPartRadio.isChecked()){ PosePtr pose = p->get(); if(pose){ bool match = false; for(size_t i=0; i < selectedLinkIndices.size(); ++i){ int linkIndex = selectedLinkIndices[i]; if(pose->isJointValid(body->link(linkIndex)->jointId) || pose->ikLinkInfo(linkIndex)){ match = true; break; } } if(match){ selectedPoseIters.insert(p); } } } else { selectedPoseIters.insert(p); } ++p; } updateLinkTreeModel(); onSelectedPosesModified(); } void PoseSeqViewBase::onAdjustStepPositionsActivated() { if(currentPoseSeqItem && currentBodyItem){ PoseSeq::iterator origin; if(selectedPoseIters.size() == 1){ origin = *selectedPoseIters.begin(); } else { origin = seq->begin(); } LeggedBody* legged = dynamic_cast(body.get()); if(legged){ int n = legged->numFeet(); vector footLinkIndices(n); for(int i=0; i < n; ++i){ footLinkIndices[i] = legged->footInfo(i).link->index; } adjustStepPositions(seq, footLinkIndices, origin); doAutomaticInterpolationUpdate(); } } } void PoseSeqViewBase::onRotateYawOrientationsActivated() { yawOrientationRotationDialog->show(); } void PoseSeqViewBase::onYawOrientationRotationDialogAccepted() { if(currentPoseSeqItem && selectedPoseIters.size() == 1){ PoseSeq::iterator poseIter = *selectedPoseIters.begin(); Vector3 center(yawOrientationRotationDialog->centerPosSpins[0].value(), yawOrientationRotationDialog->centerPosSpins[1].value(), 0.0); double angle = radian(yawOrientationRotationDialog->angleSpin.value()); rotateYawOrientations(seq, poseIter, center, angle); /* PosePtr pose = poseIter->get(); if(pose){ const std::vector& selectedLinkIndices = LinkSelectionView::mainInstance()->getSelectedLinkIndices(currentBodyItem); if(selectedLinkIndices.size() == 1){ Pose::LinkInfo* linkInfo = pose->ikLinkInfo(selectedLinkIndices.front()); if(linkInfo){ double angle = radian(yawOrientationRotationDialog->angleSpin.value()); rotateYawOrientations(seq, ++poseIter, linkInfo->p, angle); } } } */ } } void PoseSeqViewBase::onAdjustWaistPositionActivated() { linkPositionAdjustmentDialog->show(); } void PoseSeqViewBase::onLinkPositionAdjustmentDialogAccepted() { if(currentPoseSeqItem && currentBodyItem && !selectedPoseIters.empty()){ LeggedBody* legged = dynamic_cast(body.get()); if(legged){ int waistLinkIndex = currentBodyItem->body()->rootLink()->index; int n = legged->numFeet(); vector footLinkIndices(n); for(int i=0; i < n; ++i){ footLinkIndices[i] = legged->footInfo(i).link->index; } currentPoseSeqItem->beginEditing(); for(PoseIterSet::iterator p = selectedPoseIters.begin(); p != selectedPoseIters.end(); ++p){ PosePtr pose = (*p)->get(); if(pose){ seq->beginPoseModification(*p); /// \todo arbitrar selected link should be processed here Pose::LinkInfo* waistInfo = pose->ikLinkInfo(waistLinkIndex); if(waistInfo){ for(int i=0; i < 3; ++i){ if(linkPositionAdjustmentDialog->targetAxisCheck[i].isChecked()){ double p = linkPositionAdjustmentDialog->positionSpin[i].value(); if(linkPositionAdjustmentDialog->absoluteRadio.isChecked()){ waistInfo->p[i] = p; } else { waistInfo->p[i] += p; } } } } seq->endPoseModification(*p); } } currentPoseSeqItem->endEditing(); doAutomaticInterpolationUpdate(); } } } void PoseSeqViewBase::onUpdateKeyposesWithBalancedTrajectoriesActivated() { if(currentPoseSeqItem){ ostringstream mout; if(currentPoseSeqItem->updateKeyPosesWithBalancedTrajectories(mout)){ MessageView::mainInstance()->notify( _("Original key poses have been updated to be balanced ones.")); } else { MessageView::mainInstance()->notify( _("Operation failed ! Key poses cannot be updated.")); } if(!mout.str().empty()){ os << mout.str() << endl; } } } void PoseSeqViewBase::onFlipPosesActivated() { if(currentPoseSeqItem && currentBodyItem){ MessageView::mainInstance()->notify(_("flipping all the poses against x-z plane ...")); flipPoses(seq, body); doAutomaticInterpolationUpdate(); } } void PoseSeqViewBase::countSelectedKeyPoses() { MessageView::mainInstance()->notify( format(_("The number of selected key poses is %1%")) % selectedPoseIters.size()); } void PoseSeqViewBase::onSelectedPosesModified() { if(selectedPoseIters.empty()){ updateButton.setEnabled(false); deleteButton.setEnabled(false); } else { updateButton.setEnabled(true); deleteButton.setEnabled(true); } } void PoseSeqViewBase::setCurrentItemName(ItemPtr item) { if(!item || item->name().empty()){ currentItemLabel.setText(textForEmptyName); } else { currentItemLabel.setText(item->name().c_str()); } } /** \todo Show visual effect to emphasize the key poses beging updated */ void PoseSeqViewBase::onBodyKinematicStateEdited() { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::onBodyKinematicStateEdited()" << endl; } if(autoUpdateModeCheck.isChecked()){ for(PoseIterSet::iterator p = selectedPoseIters.begin(); p != selectedPoseIters.end(); ++p){ if((timeScale * (*p)->time()) == timeBar->time()){ setCurrentBodyStateToSelectedPoses(!updateAllToggle.isChecked()); InfoBar::instance()->notify(_("Selected key poses have been updated.")); break; } } } } PoseSeq::iterator PoseSeqViewBase::insertPose() { PoseSeq::iterator poseIter = seq->end(); PosePtr pose = new Pose(body->numJoints()); bool hasValidPart = false; for(int i=0; i < body->numLinks(); ++i){ Link* link = body->link(i); LinkTreeItem* item = linkTreeWidget->itemOfLink(link->index); if(item){ if(link->jointId >= 0 && isChecked(item, validPartColumn)){ pose->setJointPosition(link->jointId, link->q); hasValidPart = true; if(isChecked(item, stationaryPointColumn)){ pose->setJointStationaryPoint(link->jointId); } } if(possibleIkLinkFlag[link->index]){ if(isChecked(item, validPartColumn) || isChecked(item, ikPartColumn)){ Pose::LinkInfo* info = pose->addIkLink(link->index); info->setStationaryPoint(isChecked(item, stationaryPointColumn)); setCurrentLinkStateToIkLink(link, info); if(isChecked(item, baseLinkColumn)){ pose->setBaseLink(link->index, link->p, link->R); } if(!isChecked(item, ikPartColumn)){ info->setSlave(true); } hasValidPart = true; } } } } if(isChecked(zmpRow, validPartColumn)){ pose->setZmp(currentBodyItem->zmp()); hasValidPart = true; if(isChecked(zmpRow, stationaryPointColumn)){ pose->setZmpStationaryPoint(); } } if(hasValidPart){ poseIter = insertPoseUnit(pose); } else { showWarningDialog(_("Please check parts needed for making a desired pose.")); } return poseIter; } PoseSeq::iterator PoseSeqViewBase::insertPronunSymbol() { PronunSymbolPtr pronun(new PronunSymbol()); return insertPoseUnit(pronun); } PoseSeq::iterator PoseSeqViewBase::insertPoseUnit(PoseUnitPtr poseUnit) { PoseSeq::iterator poseIter = seq->insert(currentPoseIter, currentTime / timeScale, poseUnit); poseIter->setMaxTransitionTime(transitionTimeSpin.value() / timeScale); doAutomaticInterpolationUpdate(); toggleSelection(poseIter, false, false); currentPoseIter = poseIter; return poseIter; } void PoseSeqViewBase::onUpdateButtonClicked() { setCurrentBodyStateToSelectedPoses(!updateAllToggle.isChecked()); } void PoseSeqViewBase::setCurrentBodyStateToSelectedPoses(bool onlySelected) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::setCurrentBodyStateToSelectedPoses()" << endl; } if(body){ if(!selectedPoseIters.empty()){ bool updated = false; currentPoseSeqItem->beginEditing(); for(PoseIterSet::iterator p = selectedPoseIters.begin(); p != selectedPoseIters.end(); ++p){ PosePtr pose = (*p)->get(); if(pose){ seq->beginPoseModification(*p); if(setCurrentBodyStateToPose(pose, onlySelected)){ updated = true; seq->endPoseModification(*p); } } } currentPoseSeqItem->endEditing(updated); //! \todo This should be processed by PoseSeq slots if(updated){ doAutomaticInterpolationUpdate(); } } } } bool PoseSeqViewBase::setCurrentBodyStateToPose(PosePtr& pose, bool onlySelected) { const dynamic_bitset<>& linkSelection = LinkSelectionView::mainInstance()->getLinkSelection(currentBodyItem); bool updated = false; int n = pose->numJoints(); for(int i=0; i < n; ++i){ if(pose->isJointValid(i)){ Link* joint = body->joint(i); if(!onlySelected || linkSelection[joint->index]){ const double q = body->joint(i)->q; if(q != pose->jointPosition(i)){ pose->setJointPosition(i, q); updated = true; } } } } for(Pose::LinkInfoMap::iterator it = pose->ikLinkBegin(); it != pose->ikLinkEnd(); ++it){ const int linkIndex = it->first; Link* link = body->link(linkIndex); if(link && (!onlySelected || linkSelection[link->index])){ updated |= setCurrentLinkStateToIkLink(link, &it->second); } } if(pose->isZmpValid()){ const Vector3& zmp = currentBodyItem->zmp(); if(zmp != pose->zmp()){ pose->setZmp(zmp); updated = true; } } return updated; } /** @return true if state is modified */ bool PoseSeqViewBase::setCurrentLinkStateToIkLink(Link* link, Pose::LinkInfo* linkInfo) { bool updated = false; if(linkInfo->p != link->p){ linkInfo->p = link->p; updated = true; } if(linkInfo->R != link->R){ linkInfo->R = link->R; updated = true; } std::vector collidingLinkPairs; bool collided = false; std::vector& coldetPairs = currentBodyItem->worldColdetPairsOfLink(link->index); for(size_t i=0; i < coldetPairs.size(); ++i){ if(!coldetPairs[i]->collisions().empty()){ collided = true; break; } } if(collided){ /** \todo set a parting direction correctly (now it is assumed that the touching only happens for the flat and level floor). */ Vector3 partingDirection(0.0, 0.0, 1.0); if(!linkInfo->isTouching() || linkInfo->partingDirection() != partingDirection){ linkInfo->setTouching(partingDirection); updated = true; } } else { if(linkInfo->isTouching()){ linkInfo->clearTouching(); updated = true; } } return updated; } void PoseSeqViewBase::onDeleteButtonClicked() { cutSelectedPoses(); } void PoseSeqViewBase::onPoseInserted(PoseSeq::iterator it, bool isMoving) { if(isSelectedPoseMoving && isMoving){ selectedPoseIters.insert(it); isSelectedPoseMoving = false; onSelectedPosesModified(); } } void PoseSeqViewBase::onPoseRemoving(PoseSeq::iterator it, bool isMoving) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::onPoseRemoving(): isMoving = " << isMoving << endl; } if(it == currentPoseIter){ if(currentPoseIter != seq->begin()){ --currentPoseIter; } else if(currentPoseIter != seq->end()){ ++currentPoseIter; } } PoseIterSet::iterator p = findPoseIterInSelected(it); if(p != selectedPoseIters.end()){ selectedPoseIters.erase(p); if(isMoving){ isSelectedPoseMoving = true; } else { onSelectedPosesModified(); } } } /** @todo update currentBodyItem and call notifyUpdate in this function ? */ void PoseSeqViewBase::onPoseModified(PoseSeq::iterator it) { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::onPoseModified()" << endl; } if(!selectedPoseIters.empty() && it == *selectedPoseIters.begin()){ updateLinkTreeModel(); onSelectedPosesModified(); } } void PoseSeqViewBase::updateLinkTreeModel() { if(TRACE_FUNCTIONS){ cout << "PoseSeqViewBase::updateLinkTreeModel()" << endl; } PosePtr pose; for(PoseIterSet::iterator p = selectedPoseIters.begin(); p != selectedPoseIters.end(); ++p){ pose = (*p)->get(); if(pose){ break; } } if(!pose){ pose = poseForDefaultStateSetting; } linkTreeAttributeChangeConnections.block(); // Probably this set of block / unblock is not needed int n = linkTreeWidget->topLevelItemCount(); for(int i=0; i < n; ++i){ LinkTreeItem* item = dynamic_cast(linkTreeWidget->topLevelItem(i)); if(item){ updateLinkTreeModelSub(item, linkTreeWidget->bodyItem()->body(), *pose); } } linkTreeAttributeChangeConnections.unblock(); } PoseSeqViewBase::ChildrenState PoseSeqViewBase::updateLinkTreeModelSub (LinkTreeItem* item, Body* body, const Pose& pose) { ChildrenState state; int n = item->childCount(); for(int i=0; i < n; ++i){ LinkTreeItem* childItem = dynamic_cast(item->child(i)); if(childItem){ ChildrenState childrenState = updateLinkTreeModelSub(childItem, body, pose); state.validChildExists |= childrenState.validChildExists; state.allChildrenAreValid &= childrenState.allChildrenAreValid; state.childWithStationaryPointExists |= childrenState.childWithStationaryPointExists; state.allChildrenAreStationaryPoints &= childrenState.allChildrenAreStationaryPoints; } } if(item == zmpRow){ if(pose.isZmpValid()){ setChecked(item, validPartColumn, true); setChecked(item, stationaryPointColumn, pose.isZmpStationaryPoint()); } else { setChecked(item, validPartColumn, false); setChecked(item, stationaryPointColumn, false); } } else { Link* link = item->link(); if(link){ bool isBaseLink = false; bool isValidPart = false; bool isStationaryPoint = false; bool isIkPart = false; const Pose::LinkInfo* linkInfo = pose.ikLinkInfo(link->index); if(linkInfo){ isValidPart = true; if(!possibleIkLinkFlag[link->index]){ /// \todo put warning here or do the following call ? //pose.removeIkLink(link->index); } else if(!linkInfo->isSlave()){ isIkPart = true; isBaseLink = linkInfo->isBaseLink(); if(linkInfo->isStationaryPoint()){ isStationaryPoint = true; } } } int jointId = link->jointId; if(jointId >= 0){ if(pose.isJointValid(jointId)){ isValidPart = true; if(pose.isJointStationaryPoint(jointId)){ isStationaryPoint = true; } } } if(isValidPart && !isStationaryPoint){ state.allChildrenAreStationaryPoints = false; } if(!isValidPart){ state.allChildrenAreValid = false; } setChecked(item, baseLinkColumn, isBaseLink); setChecked(item, validPartColumn, isValidPart); setChecked(item, stationaryPointColumn, isStationaryPoint); setChecked(item, ikPartColumn, isIkPart); state.validChildExists = isValidPart; state.childWithStationaryPointExists |= isStationaryPoint; } else { if(state.allChildrenAreValid){ setChecked(item, validPartColumn, true); } else if(state.validChildExists){ setCheckState(item, validPartColumn, Qt::PartiallyChecked); } else { setChecked(item, validPartColumn, false); } if(state.allChildrenAreStationaryPoints && state.childWithStationaryPointExists){ setChecked(item, stationaryPointColumn, true); } else if(state.childWithStationaryPointExists){ setCheckState(item, stationaryPointColumn, Qt::PartiallyChecked); } else { setChecked(item, stationaryPointColumn, false); } } } return state; } void PoseSeqViewBase::doAutomaticInterpolationUpdate() { BodyMotionGenerationBar* generationBar = BodyMotionGenerationBar::instance(); if(generationBar->isAutoInterpolationUpdateMode()){ currentPoseSeqItem->updateInterpolation(); // not needed ? if(generationBar->isAutoGenerationMode()){ currentPoseSeqItem->updateTrajectory(); } } } bool PoseSeqViewBase::storeState(Archive& archive) { archive.writeItemId("currentPoseSeqItem", currentPoseSeqItem); archive.write("defaultTransitionTime", transitionTimeSpin.value()); archive.write("updateAll", updateAllToggle.isChecked()); archive.write("autoUpdate", autoUpdateModeCheck.isChecked()); archive.write("timeSync", timeSyncCheck.isChecked()); linkPositionAdjustmentDialog->storeState(archive); return linkTreeWidget->storeState(archive); } bool PoseSeqViewBase::restoreState(const Archive& archive) { if(linkTreeWidget->restoreState(archive)){ transitionTimeSpin.setValue(archive.get("defaultTransitionTime", transitionTimeSpin.value())); updateAllToggle.setChecked(archive.get("updateAll", updateAllToggle.isChecked())); autoUpdateModeCheck.setChecked(archive.get("autoUpdate", autoUpdateModeCheck.isChecked())); timeSyncCheck.setChecked(archive.get("timeSync", timeSyncCheck.isChecked())); linkPositionAdjustmentDialog->restoreState(archive); PoseSeqItem* item = archive.findItem("currentPoseSeqItem"); if(item){ setCurrentPoseSeqItem(item); } return true; } return false; } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PoseSeqViewBase.h000066400000000000000000000164711207742442300233020ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_CHOREOGRAPHY_POSE_SEQ_VIEW_BASE_H_INCLUDED #define CNOID_CHOREOGRAPHY_POSE_SEQ_VIEW_BASE_H_INCLUDED #include "PoseSeqItem.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace cnoid { class PoseSelectionDialog; /// \todo this should be independent ? class LinkPositionAdjustmentDialog; /// \todo this should be independent ? class YawOrientationRotationDialog; class PoseSeqViewBase : public boost::signals::trackable { public: PoseSeqViewBase(View* view); ~PoseSeqViewBase(); View* view; std::ostream& os; QString textForEmptyName; PoseSeqItemPtr currentPoseSeqItem; PoseSeqPtr seq; bool isSelectedPoseMoving; BodyItemPtr currentBodyItem; BodyPtr body; double currentTime; double timeScale; PoseSeq::iterator currentPoseIter; struct PoseIterTimeComp { bool operator()(const PoseSeq::iterator it1, const PoseSeq::iterator it2) { return it1->time() < it2->time(); } }; typedef std::multiset PoseIterSet; PoseIterSet selectedPoseIters; PoseSeqPtr copiedPoses; ConnectionSet poseSeqConnections; boost::signals::connection connectionOfBodyKinematicStateEdited; ConnectionSet linkTreeAttributeChangeConnections; TimeBar* timeBar; boost::signals::connection connectionOfTimeChanged; LinkTreeWidget* linkTreeWidget; int baseLinkColumn; ButtonGroup* baseLinkRadioGroup; int validPartColumn; int stationaryPointColumn; int ikPartColumn; boost::dynamic_bitset<> possibleIkLinkFlag; LinkTreeItem* zmpRow; PosePtr poseForDefaultStateSetting; QLabel currentItemLabel; CheckBox timeSyncCheck; ToolButton insertPoseButton; ToolButton updateButton; ToggleToolButton updateAllToggle; ToolButton deleteButton; CheckBox autoUpdateModeCheck; DoubleSpinBox transitionTimeSpin; struct ChildrenState { ChildrenState() : validChildExists(false), allChildrenAreValid(true), childWithStationaryPointExists(false), allChildrenAreStationaryPoints(true) { } bool validChildExists; bool allChildrenAreValid; bool childWithStationaryPointExists; bool allChildrenAreStationaryPoints; }; Menu popupMenu; MenuManager menuManager; PoseSelectionDialog* poseSelectionDialog; LinkPositionAdjustmentDialog* linkPositionAdjustmentDialog; YawOrientationRotationDialog* yawOrientationRotationDialog; PoseSeq::iterator insertPose(); PoseSeq::iterator insertPronunSymbol(); PoseSeq::iterator insertPoseUnit(PoseUnitPtr poseUnit); PoseIterSet::iterator findPoseIterInSelected(PoseSeq::iterator poseIter); bool toggleSelection(PoseSeq::iterator poseIter, bool adding, bool changeTime); void selectAllPoses(); void selectAllPosesAfterCurrentPosition(); void selectAllPosesBeforeCurrentPosition(); void selectPosesHavingSelectedLinks(); void selectPosesJustHavingSelectedLinks(); void removeSelectedPartsFromKeyPoses(); void doAutomaticInterpolationUpdate(); void updateLinkTreeModel(); bool deleteSelectedPoses(); bool cutSelectedPoses(); bool copySelectedPoses(); bool pasteCopiedPoses(double timeToPaste); bool moveSelectedPoses(double time0); bool modifyTransitionTimeOfSelectedPoses(double ttime); void popupContextMenu(QMouseEvent* event); void onSelectSpecifiedKeyPosesActivated(); void onPoseSelectionDialogAccepted(); void onAdjustStepPositionsActivated(); void onRotateYawOrientationsActivated(); void onYawOrientationRotationDialogAccepted(); void onAdjustWaistPositionActivated(); void onLinkPositionAdjustmentDialogAccepted(); void onUpdateKeyposesWithBalancedTrajectoriesActivated(); void onFlipPosesActivated(); void countSelectedKeyPoses(); virtual void onLinkTreeUpdateRequest(bool isInitialCreation); virtual void setCurrentPoseSeqItem(PoseSeqItemPtr poseSeqItem); virtual void onTimeScaleChanged(); virtual void onSelectedPosesModified(); virtual void onDeleteButtonClicked(); virtual void onPoseInserted(PoseSeq::iterator it, bool isMoving); virtual void onPoseRemoving(PoseSeq::iterator it, bool isMoving); virtual void onPoseModified(PoseSeq::iterator it); virtual bool onTimeChanged(double time) = 0; virtual void onInsertPoseButtonClicked() = 0; virtual bool restoreState(const Archive& archive); virtual bool storeState(Archive& archive); void onViewActivated(); void onViewDeactivated(); void onTimeSyncCheckToggled(); void setupOperationParts(); void setupLinkTreeWidget(); bool isChecked(LinkTreeItem* item, int column); void setChecked(LinkTreeItem* item, int column, bool checked); void setCheckState(LinkTreeItem* item, int column, Qt::CheckState state); void initializeLinkTree(); void initializeLinkTreeIkLinkColumn(); void initializeLinkTreeTraverse(QTreeWidgetItem* parentItem); void togglePoseAttribute(boost::function toggleFunction); void onBaseLinkRadioClicked(); bool setBaseLink(PosePtr& pose, Link* link); void onValidPartCheckClicked(LinkTreeItem* item, Qt::CheckState checkState); bool toggleZmp(PosePtr& pose, bool on); bool toggleLink(PosePtr& pose, LinkTreeItem* item, Link* link, bool partOn, bool ikOn); bool togglePart(PosePtr& pose, LinkTreeItem* item, bool on); void onStationaryPointCheckClicked(LinkTreeItem* linkTreeItem, Qt::CheckState checkState); bool toggleZmpStationaryPoint(PosePtr& pose, bool on); bool toggleStationaryPoint(PosePtr& pose, Link* link, bool on); bool togglePartStationaryPoints(PosePtr& pose, LinkTreeItem* item, bool on); void onIkPartCheckClicked(LinkTreeItem* item, Qt::CheckState checkState); void onInterpolationParametersChanged(); void onItemSelectionChanged(const ItemList& selectedItems); void setCurrentItemName(ItemPtr item); void onBodyKinematicStateEdited(); void onUpdateButtonClicked(); void setCurrentBodyStateToSelectedPoses(bool onlySelected); bool setCurrentBodyStateToPose(PosePtr& pose, bool onlySelected); bool setCurrentLinkStateToIkLink(Link* link, Pose::LinkInfo* linkInfo); ChildrenState updateLinkTreeModelSub(LinkTreeItem* item, Body* body, const Pose& pose); }; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PronunSymbol.cpp000066400000000000000000000013231207742442300232650ustar00rootroot00000000000000/** @file @author Shin'ichiro NAKAOKA */ #include "PronunSymbol.h" #include #include using namespace std; using namespace cnoid; PronunSymbol::PronunSymbol() { } PronunSymbol::PronunSymbol(const PronunSymbol& org) : PoseUnit(org), actualPoseUnit_(org.actualPoseUnit_) { } PronunSymbol::~PronunSymbol() { } PoseUnit* PronunSymbol::duplicate() { return new PronunSymbol(*this); } bool PronunSymbol::restore(const YamlMapping& archive, const BodyPtr body) { return true; } void PronunSymbol::store(YamlMapping& archive, const BodyPtr body) const { archive.write("type", "PronunSymbol"); archive.write("name", name(), YAML_DOUBLE_QUOTED); } choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/PronunSymbol.h000066400000000000000000000014171207742442300227360ustar00rootroot00000000000000 #ifndef CNOID_CHOREOGRAPHY_PRONUN_SYMBOL_H_INCLUDED #define CNOID_CHOREOGRAPHY_PRONUN_SYMBOL_H_INCLUDED #include "Pose.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT PronunSymbol : public PoseUnit { public: PronunSymbol(); PronunSymbol(const PronunSymbol& org); virtual ~PronunSymbol(); virtual PoseUnit* duplicate(); virtual bool restore(const YamlMapping& archive, const BodyPtr body); virtual void store(YamlMapping& archive, const BodyPtr body) const; inline PoseUnitPtr actualPoseUnit(){ return actualPoseUnit_; } private: PoseUnitPtr actualPoseUnit_; }; typedef boost::intrusive_ptr PronunSymbolPtr; } #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/exportdecl.h000066400000000000000000000004571207742442300224430ustar00rootroot00000000000000 #ifdef CNOID_EXPORT #undef CNOID_EXPORT #endif #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) # ifdef CnoidPoseSeqPlugin_EXPORTS # define CNOID_EXPORT __declspec(dllexport) # else # define CNOID_EXPORT __declspec(dllimport) # endif #else # define CNOID_EXPORT #endif choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/icons/000077500000000000000000000000001207742442300212265ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/icons/auto-update.png000066400000000000000000000022761207742442300241730ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<;IDATH‰µ–oh•eÆ÷û眹íŒmzÖšRN7h*è˜3 a…•D”õE?DÔ‡¢È±±9“ˆ)Š)D †à‡Ôab)[ŸÄMD1ü³ÜœžÉ,bµ³³óœ÷Ï݇µÃÙœIvÃÏûr¿×Åu_Ï}?¯¨*óÖ¼¢΃ÚDV<§P)ð°Øv©A·Â±VÕú^îW¢="qß²ÚUu{aQQPki<î:‘ý/z“©TÄqݾç}Ъzô´‹¼nYÖ×E±Xä™çŸwW64€ë‚ãäÖð­[\èéÑ‹ÝÝ{>jUõH°KdƒŠt?ÙØholjÂŽF§@ó ¦÷®Ë¯½½ëìô5 ÏøþÓï«NäãÍ0¹M¤ Ë:ZWS#MëÖaû>3cù3žW54°££Ãq\wí|üŸ :çDiqñ¦[·ºî‚ຌ¥ÓüxæLp;‘³Æ8¡ª<²|¹¿¾©É©]½:§ª·§‡S‡y†uͪs*@uUíâÅ®†„™ gÏãË‚›7ûM&óv¨ú’À ÃCC]ßïßÏ/ÇO)Éfy|ýzÊ+*PÛÞw_í";]e±X6ÉXÆ÷Õ…ÏgØ&ò¦À7¯lÛ&Õ׃ãp©¯cuªÑ­ªÁ= ZTw›G“ɽÆó>tT—¶¨~:×éhUýË:üsWW–LŒaay9ªj÷CÕtÞ=Ö¢z89ûý\!aØ9::újflŒ‚XŒ²X 9‚/D¢Á{ *!jÞ^à÷Õfqü 09>NA$BÔʤb†‚w Û»ŠǶÃP•Dãyn¨ŠÂ³³(¬‹º®W‰¸ÃíááiÐs3K¤ª¶en¬¬|hmee®‘þ0†çϪúI«êOùàm"‹,Ëj^U[ëØþ”Eýý8ŽÓ¿ÓóîLçå4 Œ$ÿ5 c3Ž\¾¬AúœÈ侀EN—Æ›êëc&'¹zíšçûþñüÜœÉ$’ƬÁC,ÇaKuµœL$ìD2yv·e]òU/Ø"KC‘§ªÊÊØÒØè,0†S½½:–Jy@眡jb<› î$“ö€1l¨¨ Òuy«¦ÆL§H¥ÖŒLL¬,/,t«ãqV.Y2ÕÅÆpyp¾«WxmöÏ?¦##Ù,ßݽëûªö`:í¿\Uå°,eYq18Ž›vÆ}œ¾r%>›klçFÒª¶‡ö$Œ9òÕÐPõ¦E‹Üš’b†„AÀP*ÅÉÁAotr’PuÏ hŸ y£¢]d³Â‹ÀöVÕ°M¤Ð‚} o(DÊ\×,ŒDì?=/÷¼H¨*–H—¨¾Û¬zs.ðm"Å­0Á¬ ¢M$bÁ –¿ \·àJ³êõûßC0_1ïÿÅDþãDOÃIEND®B`‚choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/icons/balancer.png000066400000000000000000000022641207742442300235070ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<1IDATH‰¥•]le†Ÿ33ÝvËÖ–¦t©(’ìBCÿ"¨FRôBPïTŒ1ÜMiü!@xhBƒÑD 7$ÞˆbBeÓBlÊqù1¥-­]Úýigwæxáv3Ð- ¾É—Lμßûž9ïäûDU¹ü~·#“1^ÈòTUTýç™^ Yw¦ýr?ŸOšš&¬P(}5•’ÁdҌŌ¡Ë— ¬hÔÚ øU™˜i¿uOõ,.tRÍÍ©U¢ª5 ¢‡•ø¢Ñûo7Äàÿàƒv‘ù;Dºÿ«ØN‘Óí"{k¹ ÚEŒt;zWõ¡)‚ϧáp&PVæŒLNc©”Œ§R2>:*ÆíÛf3ž öˆ 0<µmªiðdP íË!|âÞÒiyÿÊ•‚v‘‚€ë2 ´*û'¥O¼›`? NÂ>àí܈>©/ƒ·!çË{2inNÅ׬Iý¾b…}fñbû¸*åÀow“ëÁ„‹¬°öŠøpìe¨  ?éP&ªéÜÚËzÖ­‹÷WVêyו>о––Ê eú‡„"±² ¶@Å8Ü.²ÌRøb-Ì+Ívð&Ìv`6Àw0<Ó•½¬ÏêúÃüÌðÏCÕqøÆraí(Ì¥î FòÊÞ /ß‹XË …NBì´þNø{­wTîy­ž _ÁÈ(Œ¹`¤AF¨I$ÆcÉÚ×ÜYPú:”ÿDàZv¢ªl‡ŸÃ€ºnª*S ´Tëë'†jj&/†Ãé³UUéïAmÐW¼Ü½ð§‚„ÁíP­ªä^î‚÷~‚1¯è"z 4!¢½†á1 ý4:ýÔkð+$vÃî©Zî¨H®n¸> Å"<*ÂP(s£´ÔýZ•Å®+×¥X :ËÒ/VžpÀ÷3ÜLÀ¶)ÝœA›ª›„M6$²¥õ†¶¶ÆŽwt ﺀeÀ’‚µ÷ïùáèÑ[À%`€ ãqxµMÕžfð¡jï6˜`šlZºÔŽ——»™ž_PœÎ¥Óâ»pÁ×4TU9§€E°l·©žõjN?®UU„ ãðÔÊ•“ À­¿¿³Bˆú^ÓÉIEND®B`‚choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/icons/icons.svg000066400000000000000000001040411207742442300230620ustar00rootroot00000000000000 Icons of Choreonoid PoseSeqPlugin image/svg+xml Icons of Choreonoid PoseSeqPlugin Shin'ichiro Nakaoka choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/icons/trajectory-generation.png000066400000000000000000000026731207742442300262630ustar00rootroot00000000000000‰PNG  IHDRàw=øsBIT|dˆ pHYsììu85tEXtSoftwarewww.inkscape.org›î<8IDATH‰µS}lu~~×Ïë®·Ò––ì›Íá>ÈŽ ”ÊalLˆñ#˜ù‘H ò‡G$„ŒD3‰DB¼c$BÎÅÌ`ˆ ¬3nCt¸á:ʺv]×µ½ûù‡m2—Qb¢ï_ï½ïsïsï{ÏC(¥˜) ±r5\QÁ»ÝnOss³À·Ûí&úˆ¨ªU1ºÈDUíygμ“èqÒ÷PçöíûtÙØØ3f©:]§ª×ÿ ;·¥e;[oœ’CSâ÷àhoßB&îihø*j±Hé½½ÏU-ìö« Ä`µ,˶ŽŽuŽ«W_¥ 3qs×®ã99¬­³óqÊ0ƒÕÕ:Y–C¨‘e9$Ëò|x€ý€vrÜIQó(¥èb˜A@=ÀbQ³DQ|ŒRŠ³y¢~LÅ9G,ñ^@/TU=O)…(ŠvQçSJA) | Jxˆ™Í—Ö&OÚpø…Dý“b0¤øøÞÞÚ©'ê&ëš*,Œg¤¥ª*@€> j^nmÕZ,ôóµkcZ­–úûé[­­ú°Ñˆ÷V­šd9ŽFâìîÆë×®é¼v;=°ti!„7ÚÛ5l4 æBQ÷Ù’%i‚ ” ‚PÀ’]\\Bb™££´VQ– ‚Pþf[Û^Ð gUB,‚ Ì+++[œë÷;AÈ„Óçö˗K\.Wþš –y½‘\¿Œ$II’"<Ïxž7H’QÊËãq–½ €™+ËË9Ž3ê¥B?I’±Z­š´´4ÓÖ¾>¿¢×· Y££Õ‡ƒÌ»t©€ ÀuMFFFƒ,ËzVȲ< ÀÅú|¥ÜÝ»Ù1³™çf\¼¸ž2ŒÒÕÐp÷ZW×€ÅÖȲ<Îz½i bf³s°ªÊï¼re«ihÈ9–Ÿß†äßE1/©"Q³¾«¬¬ñÔCH¸Ód:ïè-½þkQ‹˜9ÉüÛ•+K<„D=@\¶Z·yO¨­,i4$Ä'Íõk]Ý0€“ ”Õ†Ã5èA8<ÅÁ¦d~gÆàxfæ)ÃÈÈ!Pjp¾”Ò[=€ÕââsÆ„R~DÞïÛ´‰Ižeê‰,ºïreZ»ºz4sG¡çN]݇?ö÷þ¿ŠS:ŠðIEND®B`‚choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/po/000077500000000000000000000000001207742442300205315ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/PoseSeqPlugin/po/ja.po000066400000000000000000000310471207742442300214700ustar00rootroot00000000000000# Japanese translations for PACKAGE package. # Copyright (C) 2011 THE PACKAGE'S COPYRIGHT HOLDER # This file is distributed under the same license as the PACKAGE package. # nakaoka , 2011. # msgid "" msgstr "" "Project-Id-Version: PACKAGE VERSION\n" "Report-Msgid-Bugs-To: \n" "POT-Creation-Date: 2011-12-21 18:01+0000\n" "PO-Revision-Date: 2011-11-13 23:55+0000\n" "Last-Translator: nakaoka \n" "Language-Team: Japanese\n" "Language: ja\n" "MIME-Version: 1.0\n" "Content-Type: text/plain; charset=UTF-8\n" "Content-Transfer-Encoding: 8bit\n" "Plural-Forms: nplurals=1; plural=0;\n" #: BodyMotionGenerationBar.cpp:86 msgid "Body Motion Generation Setup" msgstr "ボディモーション生æˆã®è¨­å®š" #: BodyMotionGenerationBar.cpp:91 msgid "Time scale" msgstr "タイムスケール" #: BodyMotionGenerationBar.cpp:99 msgid "Pre-initial" msgstr "開始å‰ä½™ç™½æ™‚é–“" #: BodyMotionGenerationBar.cpp:105 BodyMotionGenerationBar.cpp:114 #: BodyMotionGenerationBar.cpp:187 BodyMotionGenerationBar.cpp:202 #: BodyMotionGenerationBar.cpp:211 BodyMotionGenerationBar.cpp:221 #: PoseSeqViewBase.cpp:199 PoseSeqViewBase.cpp:206 msgid "[s]" msgstr "" #: BodyMotionGenerationBar.cpp:108 msgid "Post-final" msgstr "終了後余白時間" #: BodyMotionGenerationBar.cpp:118 msgid "Time bar's range only" msgstr "タイムãƒãƒ¼ã®ç¯„囲ã«é©ç”¨" #: BodyMotionGenerationBar.cpp:122 msgid "Put all link positions" msgstr "全リンクä½ç½®å§¿å‹¢ã‚’出力" #: BodyMotionGenerationBar.cpp:128 msgid "Make a new body item" msgstr "æ–°ã—ã„ボディアイテムã®ç”Ÿæˆ" #: BodyMotionGenerationBar.cpp:134 msgid "Stealthy Step Mode" msgstr "å¿ã³è¶³ãƒ¢ãƒ¼ãƒ‰" #: BodyMotionGenerationBar.cpp:135 msgid "" "This mode makes foot lifting / landing smoother to increase the stability" msgstr "本モードã§ã¯è¶³ã®é›¢å・接地をスムーズã«ã—ã¦å®‰å®šæ€§ã‚’å‘上ã•ã›ã¾ã™" #: BodyMotionGenerationBar.cpp:139 msgid "Height ratio thresh" msgstr "高ã•å‰²åˆã®é–¾å€¤" #: BodyMotionGenerationBar.cpp:149 msgid "Flat Lifting Height" msgstr "垂直離å終了高度" #: BodyMotionGenerationBar.cpp:156 BodyMotionGenerationBar.cpp:166 #: BodyMotionGenerationBar.cpp:177 msgid "[m]" msgstr "" #: BodyMotionGenerationBar.cpp:159 msgid "Flat Landing Height" msgstr "垂直接地開始高度" #: BodyMotionGenerationBar.cpp:170 msgid "Impact reduction height" msgstr "è¡æ’ƒç·©å’Œè»Œé“開始高度" #: BodyMotionGenerationBar.cpp:180 msgid "Impact reduction time" msgstr "è¡æ’ƒç·©å’Œè»Œé“時間長" #: BodyMotionGenerationBar.cpp:191 msgid "Auto ZMP Mode" msgstr "ZMPã®è‡ªå‹•èª¿æ•´" #: BodyMotionGenerationBar.cpp:192 msgid "Automatically insert ZMP and foot key poses for stable motion" msgstr "安定化ã®ãŸã‚ã®ZMP・足先キーãƒãƒ¼ã‚ºã®è¿½åŠ ã‚’自動ã§è¡Œã„ã¾ã™" #: BodyMotionGenerationBar.cpp:196 msgid "Min. transtion time" msgstr "最短é·ç§»æ™‚é–“" #: BodyMotionGenerationBar.cpp:205 msgid "Centering time thresh" msgstr "センタリング時間閾値" #: BodyMotionGenerationBar.cpp:215 msgid "Time margin before lifting" msgstr "足先離åå‰æ™‚間マージン" #: BodyMotionGenerationBar.cpp:227 msgid "Mix lip-sync motion" msgstr "リップシンクã®åˆæˆ" #: BodyMotionGenerationBar.cpp:237 msgid "&Ok" msgstr "了解(&O)" #: BodyMotionGenerationBar.cpp:302 msgid "Pose Seq Processing" msgstr "ãƒãƒ¼ã‚ºåˆ—処ç†" #: BodyMotionGenerationBar.cpp:304 msgid "Automatic Interpolation Update" msgstr "補完自動更新" #: BodyMotionGenerationBar.cpp:329 msgid "Generate body motions" msgstr "ボディモーションã®ç”Ÿæˆ" #: BodyMotionGenerationBar.cpp:349 msgid "Automatic Balance Adjustment Mode" msgstr "自動更新モード" #: BodyMotionGenerationBar.cpp:352 msgid "Enable the balancer" msgstr "ãƒãƒ©ãƒ³ã‚¹è£œæ­£ã®æœ‰åŠ¹åŒ–" #: FcpFileLoader.cpp:234 msgid "Choose poseset file" msgstr "ãƒãƒ¼ã‚ºå®šç¾©ãƒ•ã‚¡ã‚¤ãƒ«(.poseset)ã®é¸æŠž" #: FcpFileLoader.cpp:237 msgid "Open" msgstr "é–‹ã" #: FcpFileLoader.cpp:238 PoseSeqView.cpp:866 msgid "Cancel" msgstr "キャンセル" #: FcpFileLoader.cpp:241 msgid "FaceController poseset files (*.poseset)" msgstr "FaceControllerã®ãƒãƒ¼ã‚ºå®šç¾©ãƒ•ã‚¡ã‚¤ãƒ« (*.poseset)" #: FcpFileLoader.cpp:242 FcpFileLoader.cpp:258 msgid "Any files (*)" msgstr "å…¨ã¦ã®ãƒ•ã‚¡ã‚¤ãƒ« (*)" #: FcpFileLoader.cpp:254 msgid "Choose poseseq files" msgstr "ãƒãƒ¼ã‚ºåˆ—ファイル(.poseseq)ã®é¸æŠž" #: FcpFileLoader.cpp:257 msgid "FaceController poseseq files (*.poseseq)" msgstr "FaceControllerã®ãƒãƒ¼ã‚ºåˆ—ファイル (*.poseseq)" #: FcpFileLoader.cpp:295 msgid "FaceController Plugin Pattern Files" msgstr "FaceControllerã®ãƒ‘ターンファイル" #: PoseRollView.cpp:220 msgid "Pose Roll" msgstr "ãƒãƒ¼ã‚ºãƒ­ãƒ¼ãƒ«" #: PoseRollView.cpp:295 msgid "Select specified key poses" msgstr "特定ã®ã‚­ãƒ¼ãƒãƒ¼ã‚ºã®é¸æŠž" #: PoseRollView.cpp:297 PoseSeqViewBase.cpp:281 msgid "Adjust step positions" msgstr "ステップä½ç½®ã®èª¿æ•´" #: PoseRollView.cpp:299 msgid "Adjust waist positions of selected key poses" msgstr "é¸æŠžã‚­ãƒ¼ãƒãƒ¼ã‚ºã®è…°ä½ç½®ã®èª¿æ•´" #: PoseRollView.cpp:301 msgid "Rotate yaw orientations" msgstr "Yaw軸å‘ãã®å¤‰æ›´" #: PoseRollView.cpp:303 msgid "Update key poses with balanced trajectories" msgstr "ãƒãƒ©ãƒ³ã‚¹è£œæ­£ã•ã‚ŒãŸè»Œé“ã§ã‚­ãƒ¼ãƒãƒ¼ã‚ºã‚’æ›´æ–°" #: PoseRollView.cpp:305 msgid "Flip poses against the x-z plane" msgstr "x-zå¹³é¢ã«å¯¾ã—ã¦å転" #: PoseRollView.cpp:308 msgid "Show lip-sync elements" msgstr "リップシンクè¦ç´ ã‚’表示" #: PoseRollView.cpp:311 msgid "Menu" msgstr "メニュー" #: PoseRollView.cpp:339 msgid "Sync" msgstr "時刻åŒæœŸ" #: PoseRollView.cpp:341 PoseRollView.cpp:370 msgid "T:" msgstr "T:" #: PoseRollView.cpp:342 msgid "Current time" msgstr "ç¾åœ¨æ™‚刻" #: PoseRollView.cpp:352 msgid "Time length for editing" msgstr "編集時間長" #: PoseRollView.cpp:364 PoseRollView.cpp:381 msgid "TT:" msgstr "TT:" #: PoseRollView.cpp:371 msgid "Time of the selected pose" msgstr "é¸æŠžãƒãƒ¼ã‚ºã®æ™‚刻" #: PoseRollView.cpp:382 msgid "Transition time of the selected pose" msgstr "é¸æŠžãƒãƒ¼ã‚ºã¸ã®é·ç§»æ™‚é–“" #: PoseRollView.cpp:394 msgid "Grid:" msgstr "グリッド:" #: PoseSeqItem.cpp:40 msgid "" "Warning: the original target body %1% of \"%2%\" isdifferent from the " "current target %3%." msgstr "" "警告: 元々対象ã¨ã—ã¦ã„ãŸ\"%2%\"ã®%1%ã¯ç¾åœ¨å¯¾è±¡ã¨ã—ã¦ã„ã‚‹%3%ã¨ã¯ç•°ãªã‚Šã¾ã™ã€‚" #: PoseSeqItem.cpp:49 msgid "PoseSeqItem must be loaded as a child of a BodyItem" msgstr "" "PoseSeqアイテムã¯Bodyアイテムã®å°ã‚¢ã‚¤ãƒ†ãƒ ã¨ã—ã¦èª­ã¿è¾¼ã¾ãªã‘ã‚Œã°ãªã‚Šã¾ã›ã‚“。" #: PoseSeqItem.cpp:84 msgid "PoseSeqItem" msgstr "ãƒãƒ¼ã‚ºåˆ—アイテム" #: PoseSeqItem.cpp:87 msgid "Pose Sequence" msgstr "ãƒãƒ¼ã‚ºåˆ—" #: PoseSeqItem.cpp:89 msgid "Talk Plugin File" msgstr "Talk Plugin ファイル" #: PoseSeqItem.cpp:92 msgid "Seq File for the Face Controller" msgstr "Face Controller ã® Seq ファイル" #: PoseSeqItem.cpp:223 msgid "" "Pose seq \"%1%\" has been converted. Its target has been changed from %2% to " "%3%" msgstr "" "%2%を対象ã¨ã—ã¦ã„ãŸãƒãƒ¼ã‚ºåˆ—\"%1%\"ã¯%3%を対象ã¨ã™ã‚‹ã‚ˆã†ã«å¤‰æ›ã•ã‚Œã¾ã—ãŸã€‚" #: PoseSeqItem.cpp:589 msgid "targetBody" msgstr "対象ボディ" #: PoseSeqPlugin.cpp:44 msgid "PoseSeq Plugin Version %1%\n" msgstr "PoseSeq プラグイン ãƒãƒ¼ã‚¸ãƒ§ãƒ³ %1%\n" #: PoseSeqPlugin.cpp:46 msgid "" "This plugin has been developed by Shin'ichiro Nakaoka and Choreonoid " "Development Team, AIST, and is distributed as a part of the Choreonoid " "package.\n" "\n" msgstr "" #: PoseSeqView.cpp:169 msgid "Pose Seq" msgstr "ãƒãƒ¼ã‚ºåˆ—" #: PoseSeqView.cpp:193 msgid "Pose Seq View" msgstr "ãƒãƒ¼ã‚ºåˆ—ビュー" #: PoseSeqView.cpp:196 msgid "Normal mode" msgstr "標準モード" #: PoseSeqView.cpp:201 msgid "Humanoid mode" msgstr "ヒューマノイドモード" #: PoseSeqView.cpp:205 msgid "Lip-sync mode" msgstr "リップシンクモード" #: PoseSeqView.cpp:213 msgid "Vertical split mode" msgstr "縦分割モード" #: PoseSeqView.cpp:218 msgid "Horizontal split mode" msgstr "横分割モード" #: PoseSeqView.cpp:228 msgid "_Cut" msgstr "カット(_C)" #: PoseSeqView.cpp:230 msgid "_Copy" msgstr "コピー(_C)" #: PoseSeqView.cpp:232 msgid "_Paste" msgstr "ペースト(_P)" #: PoseSeqView.cpp:234 msgid "_Move" msgstr "移動(_M)" #: PoseSeqView.cpp:242 msgid "Target Data: " msgstr "対象データ:" #: PoseSeqView.cpp:248 PoseSeqView.cpp:290 msgid "t.time" msgstr "é·ç§»æ™‚é–“" #: PoseSeqView.cpp:265 msgid "time" msgstr "時刻" #: PoseSeqView.cpp:276 PoseSeqView.cpp:375 msgid "label" msgstr "ラベル" #: PoseSeqView.cpp:284 msgid "actual pose" msgstr "実際ã®ãƒãƒ¼ã‚º" #: PoseSeqView.cpp:375 msgid "pronunciation / label" msgstr "発音 / ラベル" #: PoseSeqView.cpp:850 msgid "Move Selected Poses" msgstr "é¸æŠžãƒãƒ¼ã‚ºã®ç§»å‹•" #: PoseSeqView.cpp:851 msgid "New time of the first pose: " msgstr "先頭ãƒãƒ¼ã‚ºã®æ–°ã—ã„時刻" #: PoseSeqView.cpp:865 msgid "Ok" msgstr "了解" #: PoseSeqViewBase.cpp:77 msgid "Link Position Adjustment" msgstr "リンクä½ç½®èª¿æ•´" #: PoseSeqViewBase.cpp:84 msgid "Absolute" msgstr "直指定" #: PoseSeqViewBase.cpp:86 msgid "Relative" msgstr "相対指定" #: PoseSeqViewBase.cpp:131 msgid "Yaw Orientation Rotation" msgstr "Yaw軸回転" #: PoseSeqViewBase.cpp:137 msgid "Center:" msgstr "中心:" #: PoseSeqViewBase.cpp:151 msgid "Angle" msgstr "角度" #: PoseSeqViewBase.cpp:156 msgid "[deg]" msgstr "[deg]" #: PoseSeqViewBase.cpp:187 msgid "Select Specified Key Poses" msgstr "特定ã®ã‚­ãƒ¼ãƒãƒ¼ã‚ºã‚’é¸æŠž" #: PoseSeqViewBase.cpp:194 msgid "Start" msgstr "最å°æ™‚刻" #: PoseSeqViewBase.cpp:201 msgid "End" msgstr "最大時刻" #: PoseSeqViewBase.cpp:211 msgid "all parts" msgstr "全部ä½" #: PoseSeqViewBase.cpp:213 msgid "having selected parts" msgstr "é¸æŠžéƒ¨ä½ã‚’有ã™ã‚‹" #: PoseSeqViewBase.cpp:216 msgid "just selected parts" msgstr "é¸æŠžéƒ¨ä½ã«ä¸€è‡´" #: PoseSeqViewBase.cpp:277 msgid "Select all poses after current position" msgstr "ç¾åœ¨æ™‚刻後ã®å…¨ãƒãƒ¼ã‚ºã‚’é¸æŠž" #: PoseSeqViewBase.cpp:279 msgid "Select all poses before current position" msgstr "ç¾åœ¨æ™‚刻å‰ã®å…¨ãƒãƒ¼ã‚ºã‚’é¸æŠž" #: PoseSeqViewBase.cpp:283 msgid "Count selected key poses" msgstr "é¸æŠžãƒãƒ¼ã‚ºã®æ•°" #: PoseSeqViewBase.cpp:338 msgid " Insert " msgstr "挿入" #: PoseSeqViewBase.cpp:340 msgid "Insert a new pose at the current time position" msgstr "æ–°ã—ã„ãƒãƒ¼ã‚ºã‚’ç¾åœ¨æ™‚刻ã«æŒ¿å…¥ã™ã‚‹" #: PoseSeqViewBase.cpp:344 msgid "Transition time of a newly inserted pose" msgstr "æ–°ãŸã«æŒ¿å…¥ã•ã‚Œã‚‹ãƒãƒ¼ã‚ºã®é·ç§»æ™‚é–“" #: PoseSeqViewBase.cpp:352 msgid "Update" msgstr "æ›´æ–°" #: PoseSeqViewBase.cpp:354 msgid "Update the selected pose with the current robot state" msgstr "é¸æŠžãƒãƒ¼ã‚ºã‚’ロボットã®ç¾åœ¨å§¿å‹¢ã§æ›´æ–°" #: PoseSeqViewBase.cpp:358 msgid "All" msgstr "全部ä½" #: PoseSeqViewBase.cpp:360 msgid "The update button updates all the element of the selected pose." msgstr "é¸æŠžãƒãƒ¼ã‚ºã®å…¨ã¦ã®éƒ¨ä½ã‚’æ›´æ–°" #: PoseSeqViewBase.cpp:363 msgid "Auto" msgstr "自動更新" #: PoseSeqViewBase.cpp:364 msgid "" "The selected pose is automatically updated when the robot state changes." msgstr "ロボットã®çŠ¶æ…‹å¤‰åŒ–ã«å¿œã˜ã¦è‡ªå‹•ã§é¸æŠžå§¿å‹¢ã‚’æ›´æ–°" #: PoseSeqViewBase.cpp:367 msgid "Delete" msgstr "削除" #: PoseSeqViewBase.cpp:372 msgid "Time sync" msgstr "時刻åŒæœŸ" #: PoseSeqViewBase.cpp:417 msgid "Select key poses having the selected links" msgstr "é¸æŠžãƒªãƒ³ã‚¯ã‚’å«ã‚€ãƒãƒ¼ã‚ºã‚’é¸æŠž" #: PoseSeqViewBase.cpp:419 msgid "Select key poses just having the selected links" msgstr "é¸æŠžãƒªãƒ³ã‚¯ã«ä¸€è‡´ã™ã‚‹ãƒãƒ¼ã‚ºã‚’é¸æŠž" #: PoseSeqViewBase.cpp:421 msgid "Remove the selected parts from the selected poses" msgstr "é¸æŠžãƒãƒ¼ã‚ºã‹ã‚‰é¸æŠžéƒ¨ä½ã‚’削除" #: PoseSeqViewBase.cpp:1372 msgid "Original key poses have been updated to be balanced ones." msgstr "å…ƒã®ãƒãƒ¼ã‚ºã‚’ãƒãƒ©ãƒ³ã‚¹è£œæ­£ã•ã‚ŒãŸã‚‚ã®ã«æ›´æ–°ã™ã‚‹ã€‚" #: PoseSeqViewBase.cpp:1375 msgid "Operation failed ! Key poses cannot be updated." msgstr "処ç†å¤±æ•—ï¼ãƒãƒ¼ã‚ºãŒæ›´æ–°ã§ãã¾ã›ã‚“。" #: PoseSeqViewBase.cpp:1387 msgid "flipping all the poses against x-z plane ..." msgstr "全姿勢をx-zå¹³é¢ã«å¯¾ã—ã¦å転" #: PoseSeqViewBase.cpp:1397 msgid "The number of selected key poses is %1%" msgstr "é¸æŠžãƒãƒ¼ã‚ºã®æ•°ã¯%1%ã§ã™ã€‚" #: PoseSeqViewBase.cpp:1436 msgid "Selected key poses have been updated." msgstr "é¸æŠžãƒãƒ¼ã‚ºã¯æ›´æ–°ã•ã‚Œã¾ã—ãŸã€‚" #: PoseSeqViewBase.cpp:1491 msgid "Please check parts needed for making a desired pose." msgstr "望ã¿ã®å§¿å‹¢ã‚’ã¤ãã‚‹ã®ã«å¿…è¦ãªéƒ¨ä½ã‚’é¸æŠžã—ã¦ãã ã•ã„。" choreonoid-1.1.0+dfsg/src/Util/000077500000000000000000000000001207742442300162725ustar00rootroot00000000000000choreonoid-1.1.0+dfsg/src/Util/CMakeLists.txt000066400000000000000000000036741207742442300210440ustar00rootroot00000000000000 # @author Shin'ichiro Nakaoka # set(CMAKE_BUILD_TYPE Release) configure_file(Config.h.in ${CMAKE_CURRENT_SOURCE_DIR}/Config.h) set(sources EasyScanner.cpp NullOut.cpp SeqBase.cpp Vector3Seq.cpp MultiAffine3Seq.cpp MultiValueSeq.cpp PlainSeqFormatLoader.cpp Interpolator.cpp RangeLimiter.cpp TriangleMeshShaper.cpp Triangulator.cpp ImageConverter.cpp FileUtil.cpp Utf8.cpp VrmlNodes.cpp VrmlParser.cpp VrmlWriter.cpp YamlNodes.cpp YamlReader.cpp YamlWriter.cpp EigenUtil.cpp ) set(headers EasyScanner.h GaussianFilter.h ImageConverter.h Interpolator.h MultiAffine3Seq.h MultiSeq.h MultiValueSeq.h NullOut.h PlainSeqFormatLoader.h RangeLimiter.h Referenced.h Seq.h SeqBase.h TimeMeasure.h Sleep.h TriangleMeshShaper.h Triangulator.h Vector3Seq.h FileUtil.h Utf8.h VrmlNodes.h VrmlParser.h VrmlWriter.h YamlNodes.h YamlReader.h YamlWriter.h EigenTypes.h EigenUtil.h EigenYaml.h exportdecl.h ) set(target CnoidUtil) add_library(${target} SHARED ${sources} ${headers}) if(UNIX) target_link_libraries(${target} yaml ${PNG_LIBRARY} ${JPEG_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SIGNALS_LIBRARY} ${GETTEXT_LIBRARIES} ) elseif(MSVC) set_target_properties(${target} PROPERTIES COMPILE_DEFINITIONS "YAML_DECLARE_STATIC") if(USE_EXTERNAL_YAML) target_link_libraries(${target} optimized yaml debug yamld libpng jpeg ${GETTEXT_LIBRARIES}) else() target_link_libraries(${target} yaml libpng jpeg ${GETTEXT_LIBRARIES}) endif() endif() apply_common_setting_for_library(${target} "${headers}") install_external_libraries(${Boost_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SIGNALS_LIBRARY}) if(CNOID_ENABLE_GETTEXT) install_external_libraries(${GETTEXT_BINARY_DIR} ${GETTEXT_LIBRARY_DIR} ${GETTEXT_LIBRARIES}) endif() choreonoid-1.1.0+dfsg/src/Util/Config.h.in000066400000000000000000000010601207742442300202520ustar00rootroot00000000000000 #ifndef CNOID_CONFIG_H_INCLUDED #define CNOID_CONFIG_H_INCLUDED #define CNOID_MAJOR_VERSION @CNOID_MAJOR_VERSION@ #define CNOID_MINOR_VERSION @CNOID_MINOR_VERSION@ #define CNOID_PATCH_VERSION @CNOID_PATCH_VERSION@ #define CNOID_VERSION_STRING "@CNOID_VERSION@" #define CNOID_FULL_VERSION_STRING "@CNOID_FULL_VERSION@" #define CNOID_DIR "@CNOID_DIR@" #define CNOID_PLUGIN_SUBDIR "@CNOID_PLUGIN_SUBDIR@" #define CNOID_SHARE_DIR "@CNOID_SHARE_DIR@" #define CNOID_SHARE_SUBDIR "@CNOID_SHARE_SUBDIR@" #define CNOID_LOCALE_SUBDIR "@CNOID_LOCALE_SUBDIR@" #endif choreonoid-1.1.0+dfsg/src/Util/EasyScanner.cpp000066400000000000000000000346661207742442300212300ustar00rootroot00000000000000/*! @file @brief Implementation of text scanner class @author Shin'ichiro Nakaoka */ #include "EasyScanner.h" #include #include #include #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; // Replacement for 'strtod()' function in Visual C++ // This is neccessary because the implementation of VC++6.0 uses 'strlen()' in the function, // so that it becomes too slow for a string buffer which has long length. #ifdef _MSC_VER static double mystrtod(const char* nptr, char** endptr) { const char* org = nptr; bool valid = false; double value = 0.0; double sign = +1.0; if(*nptr == '+'){ nptr++; } else if(*nptr == '-'){ sign = -1.0; nptr++; } if(isdigit((unsigned char)*nptr)){ valid = true; do { value = value * 10.0 + (*nptr - '0'); nptr++; } while(isdigit((unsigned char)*nptr)); } if(*nptr == '.'){ valid = false; nptr++; if(isdigit((unsigned char)*nptr)){ double small = 0.1; valid = true; do { value += small * (*nptr - '0'); small *= 0.1; nptr++; } while(isdigit((unsigned char)*nptr)); } } if(valid && (*nptr == 'e' || *nptr == 'E')){ nptr++; valid = false; double psign = +1.0; if(*nptr == '+'){ nptr++; } else if(*nptr == '-'){ psign = -1.0; nptr++; } if(isdigit((unsigned char)*nptr)){ valid = true; double p = 0.0; do { p = p * 10.0 + (*nptr - '0'); nptr++; } while(isdigit((unsigned char)*nptr)); value *= pow(10.0, psign * p); } } if(valid){ *endptr = (char*)nptr; } else { *endptr = (char*)org; } return sign * value; } #else static inline double mystrtod(const char* nptr, char** endptr) { return strtod(nptr, endptr); } #endif std::string EasyScanner::Exception::getFullMessage() { string m(message); if(lineNumber > 0){ m += str(format(" at line %1%") % lineNumber); } if(!filename.empty()){ m += str(format(" of %1%") % filename); } return m; } EasyScanner::EasyScanner() { init(); } /** @param filename file to read. */ EasyScanner::EasyScanner(string filename) { init(); loadFile(filename); } void EasyScanner::init() { textBuf = 0; size = 0; textBufEnd = 0; lineNumberOffset = 1; commentChar = '#'; quoteChar = 0xffff; isLineOriented = true; defaultErrorMessage = "unknown error of the lexical scanner"; whiteSpaceChars.push_back(' '); whiteSpaceChars.push_back('\t'); symbols.reset(new SymbolMap()); } /*! Copy Constructor. New object inherits another's propety and symbols. @param scanner original object @param copy_text If true, new object has same text as original */ EasyScanner::EasyScanner(const EasyScanner& org, bool copyText) : whiteSpaceChars(org.whiteSpaceChars) { commentChar = org.commentChar; quoteChar = org.quoteChar; isLineOriented = org.isLineOriented; filename = org.filename; defaultErrorMessage = org.defaultErrorMessage; lineNumber = org.lineNumber; lineNumberOffset = org.lineNumberOffset; symbols = org.symbols; if(copyText && org.textBuf){ size = org.size; textBuf = new char[size+1]; memcpy(textBuf, org.textBuf, size+1); text = textBuf; textBufEnd = textBuf + size; } else { textBuf = 0; size = 0; textBufEnd = 0; } } /*! This function directly sets a text in the main memory */ void EasyScanner::setText(const char* text, int len) { if(textBuf) delete[] textBuf; size = len; textBuf = new char[size+1]; memcpy(textBuf, text, len); textBuf[size] = 0; this->text = textBuf; textBufEnd = textBuf + size; lineNumber = lineNumberOffset; filename = ""; } EasyScanner::~EasyScanner() { if(textBuf) delete[] textBuf; } void EasyScanner::setLineNumberOffset(int offset) { lineNumberOffset = offset; } void EasyScanner::moveToHead() { text = textBuf; lineNumber = lineNumberOffset; } void EasyScanner::putSymbols() { SymbolMap::iterator p = symbols->begin(); while(p != symbols->end()){ cout << p->first << " = " << p->second << std::endl; p++; } } void EasyScanner::throwException(const char* message) { Exception ex; ex.message = message ? message : defaultErrorMessage; ex.filename = filename; ex.lineNumber = lineNumber; throw ex; } void EasyScanner::throwException(const std::string& message) { throwException(message.c_str()); } /*! This function sets the identifier character of comment beginning. @param cc Identifier character. Default is '#'. If you want no comment, set 0. */ void EasyScanner::setCommentChar(char cc) { commentChar = cc ? cc : 0xffff; } void EasyScanner::setLineOriented(bool on) { isLineOriented = on; } /*! If there is a character to ignore, you can set it by this function */ void EasyScanner::setWhiteSpaceChar(char ws) { whiteSpaceChars.push_back(ws); } /*! If you want to read quoted string, set quote character by this function. In default, this is unset. */ void EasyScanner::setQuoteChar(char qs) { quoteChar = qs; } /** This function loads a text from a given file. The function thorws EasyScanner::Exception when the file cannot be loaded. */ void EasyScanner::loadFile(const string& filename) { this->filename.clear(); FILE* file = fopen(filename.c_str(), "rb"); if(!file){ string message; switch(errno){ case ENOENT: message = filename + " cannot be found."; break; default: message = string("I/O error in accessing ") + filename; break; } throwException(message.c_str()); } this->filename = filename; fseek(file, 0, SEEK_END); size = ftell(file); rewind(file); if(textBuf) delete[] textBuf; textBuf = new char[size+1]; size = fread(textBuf, sizeof(char), size, file); textBuf[size] = 0; fclose(file); text = textBuf; textBufEnd = textBuf + size; lineNumber = lineNumberOffset; } /** move the current position to just before the end (LF or EOF) of a line */ inline void EasyScanner::skipToLineEnd() { while(*text != '\r' && *text != '\n' && *text != '\0') text++; } void EasyScanner::skipSpace() { int n = whiteSpaceChars.size(); while(true){ int i=0; while(i < n){ if(*text == whiteSpaceChars[i]){ text++; i = 0; } else { i++; } } if(*text == commentChar){ text++; skipToLineEnd(); } if(isLineOriented){ break; } if(*text == '\n'){ text++; } else if(*text == '\r'){ text++; if(*text == '\n'){ text++; } } else { break; } lineNumber++; } } /** This function does not call 'skipSpace()'. On the other hand, 'readLF()' calls 'skipSpace()' before calling 'readLF0()' */ bool EasyScanner::readLF0() { if(*text == '\n'){ text++; lineNumber++; return true; } else if(*text == '\r'){ text++; if(*text == '\n'){ text++; } lineNumber++; return true; } return false; } bool EasyScanner::checkLF() { char* current = text; if(readLF()){ text = current; return true; } return false; } int EasyScanner::readToken() { skipSpace(); if(isdigit((unsigned char)*text) || *text == '+' || *text == '-'){ char* tail; intValue = strtol(text, &tail, 0); if(tail != text){ text = tail; return T_INTEGER; } doubleValue = mystrtod(text, &tail); if(tail != text){ text = tail; return T_DOUBLE; } charValue = *text; text++; return T_SIGLUM; } else if(isalpha((unsigned char)*text)){ char* org = text; text++; while(isalnum((unsigned char)*text) || *text == '_') text++; stringValue.assign(org, text - org); if(stringValue.size() == 1){ charValue = *org; return T_ALPHABET; } else { return T_WORD; } } else if(*text == quoteChar) { return extractQuotedString() ? T_STRING : T_SIGLUM; } else if(ispunct((unsigned char)*text)){ charValue = *text; text++; return T_SIGLUM; } else if(readLF0()){ return T_LF; } else if(*text == '\0'){ return T_EOF; } return T_NONE; } /*! This function makes all the characters in stringValue lower case */ void EasyScanner::toLower() { for(size_t i=0; i < stringValue.size(); ++i){ stringValue[i] = tolower(stringValue[i]); } } bool EasyScanner::extractQuotedString() { text++; char* org = text; if(isLineOriented){ while(true){ if(*text == '\r' || *text == '\n' || *text == '\0'){ text = org; return false; } if(*text == quoteChar) break; text++; } } else { while(true){ if(*text == '\0'){ text = org; return false; } readLF0(); if(*text == quoteChar) break; text++; } } stringValue.assign(org, text - org); text++; return true; } bool EasyScanner::readDouble() { char* tail; if(checkLF()) return false; doubleValue = mystrtod(text, &tail); if(tail != text){ text = tail; return true; } return false; } bool EasyScanner::readInt() { char* tail; if(checkLF()) return false; intValue = strtol(text, &tail, 0); if(tail != text){ text = tail; return true; } return false; } bool EasyScanner::readChar() { skipSpace(); if(isgraph((unsigned char)*text)){ charValue = *text; text++; return true; } return false; } bool EasyScanner::readChar(int chara) { skipSpace(); if(*text == chara){ text++; return true; } return false; } int EasyScanner::peekChar() { skipSpace(); return *text; } bool EasyScanner::readWord0() { char* org = text; while(true){ int c = (unsigned char)*text; if(!isalnum(c) && isascii(c) && c != '_'){ break; } text++; } if(text - org > 0){ stringValue.assign(org, text - org); return true; } return false; } bool EasyScanner::readString0(const int delimiterChar) { char* org = text; while(true){ int c = (unsigned char)*text; if(isspace(c) || iscntrl(c) || c == delimiterChar){ break; } text++; } if(text - org > 0){ stringValue.assign(org, text - org); return true; } return false; } bool EasyScanner::readString(const char* str) { skipSpace(); char* org = text; while(*str != '\0'){ if(*str++ != *text++){ text = org; return false; } } return true; } /** read a quoted string. If 'allowNoQuotedWord' is true, the function read a word without quotations. */ bool EasyScanner::readQuotedString(bool allowNoQuotedWord) { skipSpace(); if(*text == quoteChar){ return extractQuotedString(); } else if(allowNoQuotedWord){ return readString0(' '); } return false; } bool EasyScanner::readUnquotedTextBlock() { skipSpace(); char* org = text; while(true){ if(*text == '\r' || *text == '\n' || *text == commentChar || *text == '\0'){ break; } text++; } if(text != org){ stringValue.assign(org, text - org); return true; } return false; } bool EasyScanner::readSymbol() { if(readWord()){ symbolValue = getSymbolID(stringValue); if(symbolValue){ return true; } } return false; } bool EasyScanner::readSymbol(int id) { char* org = text; int orglineNumber = lineNumber; if(readWord()){ symbolValue = getSymbolID(stringValue); if(symbolValue == id){ return true; } else { text = org; lineNumber = orglineNumber; } } return false; } bool EasyScanner::skipLine() { while(true){ if(readLF0()){ return true; } if(*text == '\0'){ return false; } text++; } } bool EasyScanner::readLine() { char* org = text; if(skipLine()){ // eliminate newline code char* end = text - 1; if(*end == '\n'){ end--; if(*end == '\r'){ end--; } } end++; stringValue.assign(org, end - org); return true; } return false; } bool EasyScanner::skipBlankLines() { do { if(*text == '\0'){ return false; } } while(readLF()); return true; } // operators EasyScanner& operator>>(EasyScanner& scanner, double& value) { if(!scanner.readDouble()){ scanner.throwException("scan error: can't read double value"); } value = scanner.doubleValue; return scanner; } EasyScanner& operator>>(EasyScanner& scanner, int& value) { if(!scanner.readInt()){ scanner.throwException("scan error: can't read int value"); throw scanner; } value = scanner.intValue; return scanner; } EasyScanner& operator>>(EasyScanner& scanner, const char* matchString) { scanner.skipSpace(); while(*matchString != '\0'){ if(*scanner.text++ != *matchString++){ scanner.throwException("scan error: unmatched string"); } } return scanner; } EasyScanner& operator>>(EasyScanner& scanner, char matchChar) { scanner.skipSpace(); if(*scanner.text++ != matchChar){ scanner.throwException("scan error: unmatched cahracter"); } return scanner; } EasyScanner& operator>>(EasyScanner& scanner, string& str) { scanner.skipSpace(); if(!scanner.readQuotedString(true)){ scanner.throwException("scan error: can't read string"); } str = scanner.stringValue; return scanner; } EasyScanner& operator>>(EasyScanner& scanner, EasyScanner::Endl endl) { if(!scanner.readLF()){ scanner.throwException("scan error: end of line unmatched"); } return scanner; } choreonoid-1.1.0+dfsg/src/Util/EasyScanner.h000066400000000000000000000173021207742442300206610ustar00rootroot00000000000000 /*! @file @brief The header file of a text scanner class @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_EASYSCANNER_H_INCLUDED #define CNOID_UTIL_EASYSCANNER_H_INCLUDED #include #include #include #include #include "exportdecl.h" namespace cnoid { /** @todo introduce a pimpl to hide the use of map, vector */ class CNOID_EXPORT EasyScanner { public: class Endl { //int dummy; }; class CNOID_EXPORT Exception { public: std::string message; std::string filename; int lineNumber; std::string getFullMessage(); }; enum TokenType { T_NONE = 0, T_SPACE, T_ALPHABET, T_INTEGER, T_DOUBLE, T_WORD, T_STRING, T_SIGLUM, T_LF, T_EOF }; typedef std::map SymbolMap; typedef std::pair SymbolPair; typedef boost::shared_ptr SymbolMapPtr; Endl endl; EasyScanner(); EasyScanner(std::string filename); EasyScanner(const EasyScanner& scanner, bool copy_text = false); virtual ~EasyScanner(); void putSymbols(); inline void registerSymbol(int id, const std::string& symbol) { symbols->insert(SymbolPair(symbol, id)); } inline int getSymbolID(const std::string& symbol) { SymbolMap::iterator p = symbols->find(symbol); return (p != symbols->end()) ? p->second : 0; } /// if 0, comment is disabled void setCommentChar(char cc); void setLineOriented(bool on); void setQuoteChar(char qc); void setWhiteSpaceChar(char ws); void loadFile(const std::string& filename); void setText(const char* text, int len); void setLineNumberOffset(int offset); void setDefaultErrorMessage(const std::string& message){ defaultErrorMessage = message; } void moveToHead(); int readToken(); void toLower(); bool readDouble(); bool readInt(); bool readChar(); bool readChar(int chara); int peekChar(); /** In contrast to readString(), this function does not recognize siglums except '_' as a part of a word. */ inline bool readWord() { skipSpace(); return readWord0(); } /** In contrast to readWord(), this function allows a string to include siglums such as !,",#,$,%,&,... */ inline bool readString(const int delimiterChar = ',') { skipSpace(); return readString0(delimiterChar); } bool readString(const char* str); inline bool readString(const std::string& str) { return readString(str.c_str()); } bool readQuotedString(bool allowNoQuotedWord = false); bool readUnquotedTextBlock(); bool readSymbol(); bool readSymbol(int id); inline bool isEOF(){ skipSpace(); return (*text == '\0'); } /// reading a line feed inline bool readLF() { skipSpace(); return readLF0(); } inline bool readLFEOF() { skipSpace(); return readLF0() ? true : (*text == '\0'); } bool checkLF(); bool readLine(); bool skipLine(); bool skipBlankLines(); void skipSpace(); void throwException(const char* message); void throwException(const std::string& message); /** The exception version of readInt(). \return Scanned int value. */ inline int readIntEx(const char* message = 0) { if(!readInt()) throwException(message); return intValue; } /** The exception version of readDouble(). \return Scanned double value. */ inline double readDoubleEx(const char* message = 0) { if(!readDouble()) throwException(message); return doubleValue; } /** The exception version of readChar(). \return Scanned char value. */ inline int readCharEx(const char* message = 0) { if(!readChar()) throwException(message); return charValue; } /** The exception version of readChar(). */ inline void readCharEx(int chara, const char* message = 0) { if(!readChar(chara)) throwException(message); } /** The exception version of readWord(). \return Scanned word string. */ inline std::string readWordEx(const char* message = 0) { if(!readWord()) throwException(message); return stringValue; } /** The exception version of readString(). \return Scanned word string. */ inline std::string readStringEx(const char* message = 0) { if(!readString()) throwException(message); return stringValue; } inline std::string readQuotedStringEx(const char* message = 0) { if(!readQuotedString()) throwException(message); return stringValue; } /** The exception version of readSymbol(). \return ID of the scanned symbol. */ inline int readSymbolEx(const char* message = 0) { if(!readSymbol()) throwException(message); return symbolValue; } /** The exception version of readLF(). */ inline void readLFex(const char* message = 0) { if(!readLF()) throwException(message); } inline void readLFEOFex(const char* message = 0) { if(!readLFEOF()) throwException(message); } int intValue; double doubleValue; std::string stringValue; char charValue; int symbolValue; std::string defaultErrorMessage; int lineNumber; char* text; std::string filename; private: void init(); bool extractQuotedString(); inline void skipToLineEnd(); bool readLF0(); bool readWord0(); bool readString0(const int delimiterChar); char* textBuf; int size; char* textBufEnd; int lineNumberOffset; int commentChar; int quoteChar; bool isLineOriented; std::vector whiteSpaceChars; SymbolMapPtr symbols; friend CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, double& value); friend CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, int& value); friend CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, const char* matchString); friend CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, char matchChar); friend CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, std::string& str); friend CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, EasyScanner::Endl endl); }; CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, double& value); CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, int& value); CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, const char* matchString); CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, char matchChar); CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, std::string& str); CNOID_EXPORT EasyScanner& operator>>(EasyScanner& scanner, EasyScanner::Endl endl); typedef boost::shared_ptr EasyScannerPtr; } #endif choreonoid-1.1.0+dfsg/src/Util/EigenTypes.h000066400000000000000000000013031207742442300205140ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_EIGEN_TYPES_H_INCLUDED #define CNOID_UTIL_EIGEN_TYPES_H_INCLUDED #include #include namespace cnoid { using Eigen::Matrix2d; using Eigen::Vector2d; using Eigen::Matrix3d; using Eigen::Vector3d; using Eigen::Matrix4d; using Eigen::Vector4d; using Eigen::MatrixXd; using Eigen::VectorXd; using Eigen::AngleAxisd; typedef Eigen::Matrix2d Matrix2; typedef Eigen::Vector2d Vector2; typedef Eigen::Matrix3d Matrix3; typedef Eigen::Vector3d Vector3; typedef Eigen::Matrix4d Matrix4; typedef Eigen::Vector4d Vector4; typedef Eigen::Affine3d Affine3; } #endif choreonoid-1.1.0+dfsg/src/Util/EigenUtil.cpp000066400000000000000000000022671207742442300206720ustar00rootroot00000000000000 #include "EigenUtil.h" namespace cnoid { Matrix3 rotFromRpy(double r, double p, double y) { const double cr = cos(r); const double sr = sin(r); const double cp = cos(p); const double sp = sin(p); const double cy = cos(y); const double sy = sin(y); Matrix3 R; R << cp*cy, sr*sp*cy - cr*sy, cr*sp*cy + sr*sy, cp*sy, sr*sp*sy + cr*cy, cr*sp*sy - sr*cy, -sp , sr*cp , cr*cp; return R; } Vector3 omegaFromRot(const Matrix3& R) { double alpha = (R(0,0) + R(1,1) + R(2,2) - 1.0) / 2.0; if(fabs(alpha - 1.0) < 1.0e-6) { //th=0,2PI; return Vector3::Zero(); } else { double th = acos(alpha); double s = sin(th); if (s < std::numeric_limits::epsilon()) { //th=PI return Vector3( sqrt((R(0,0)+1)*0.5)*th, sqrt((R(1,1)+1)*0.5)*th, sqrt((R(2,2)+1)*0.5)*th ); } double k = -0.5 * th / s; return Vector3((R(1,2) - R(2,1)) * k, (R(2,0) - R(0,2)) * k, (R(0,1) - R(1,0)) * k); } } } choreonoid-1.1.0+dfsg/src/Util/EigenUtil.h000066400000000000000000000022041207742442300203260ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_EIGEN_UTIL_H_INCLUDED #define CNOID_UTIL_EIGEN_UTIL_H_INCLUDED #include "EigenTypes.h" #include "EigenYaml.h" #include "exportdecl.h" namespace cnoid { const double PI = 3.14159265358979323846; const double PI_2 = 1.57079632679489661923; inline double degree(double rad) { return (180.0 * rad / PI); } inline double radian(double deg) { return (PI * deg / 180.0); } template inline Eigen::Matrix::Scalar, 3, 1> rpyFromRot(const Eigen::MatrixBase& R) { Vector3 ea = R.eulerAngles(2, 1, 0); return Vector3(ea[2], ea[1], ea[0]); } CNOID_EXPORT Matrix3 rotFromRpy(double r, double p, double y); inline Matrix3 rotFromRpy(const Vector3& rpy) { return rotFromRpy(rpy[0], rpy[1], rpy[2]); } CNOID_EXPORT Vector3 omegaFromRot(const Matrix3& R); inline Matrix3 hat(const Vector3& x) { Matrix3 M; M << 0.0, -x(2), x(1), x(2), 0.0, -x(0), -x(1), x(0), 0.0; return M; } } #endif choreonoid-1.1.0+dfsg/src/Util/EigenYaml.h000066400000000000000000000034341207742442300203210ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_YAML_EIGEN_H_INCLUDED #define CNOID_UTIL_YAML_EIGEN_H_INCLUDED #include "YamlNodes.h" #include namespace cnoid { template bool read(const YamlMapping& mapping, const std::string& key, Eigen::MatrixBase& x) { const YamlSequence& s = *mapping.findSequence(key); if(s.isValid()){ const int nr = x.rows(); const int nc = x.cols(); const int n = s.size(); int index = 0; if(n > 0){ for(int i=0; i < nr; ++i){ for(int j=0; j < nc; ++j){ x(i, j) = s[index++].toDouble(); if(index == n){ break; } } } } return (index == nr * nc); } return false; } template inline void readEx(const YamlMapping& mapping, const std::string& key, Eigen::MatrixBase& x) { if(!read(mapping, key, x)){ mapping.throwKeyNotFoundException(key); } } template YamlSequence& write(YamlMapping& mapping, const std::string& key, const Eigen::MatrixBase& x) { YamlSequence& s = *mapping.createFlowStyleSequence(key); const int nr = x.rows(); const int nc = x.cols(); if(nc == 1){ for(int i=0; i < nr; ++i){ s.append(x(i)); } } else { for(int i=0; i < nr; ++i){ s.appendLF(); for(int j=0; j < nc; ++j){ s.append(x(i, j)); } } } return s; } } #endif choreonoid-1.1.0+dfsg/src/Util/FileUtil.cpp000066400000000000000000000150361207742442300205200ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "FileUtil.h" #ifdef _WIN32 #include #include #include #endif using namespace std; using namespace boost; namespace cnoid { void makePathCompact(const filesystem::path& path, filesystem::path& out_compact) { out_compact.clear(); for(filesystem::path::const_iterator p = path.begin(); p != path.end(); ++p){ if(*p == ".."){ out_compact.remove_filename(); } else if(*p != "."){ out_compact /= *p; } } } bool findSubDirectory(const filesystem::path& directory, const filesystem::path& path, filesystem::path& out_subdirectory) { if(directory.is_complete() && path.is_complete()){ filesystem::path compactPath; makePathCompact(path, compactPath); filesystem::path::const_iterator p = directory.begin(); filesystem::path::const_iterator q = compactPath.begin(); while(p != directory.end() && q != compactPath.end()){ #ifdef _WIN32 if(to_lower_copy(*p) != to_lower_copy(*q)){ #else if(*p != *q){ #endif break; } ++p; ++q; } if(p == directory.end()){ out_subdirectory.clear(); while(q != compactPath.end()){ out_subdirectory /= *q++; } return true; } } return false; } bool findRelativePath(const filesystem::path& from_, const filesystem::path& to, filesystem::path& out_relativePath) { if(from_.is_complete() && to.is_complete()){ filesystem::path from; makePathCompact(from_, from); filesystem::path::const_iterator p = from.begin(); filesystem::path::const_iterator q = to.begin(); while(p != from.end() && q != to.end()){ #ifdef _WIN32 if(to_lower_copy(*p) != to_lower_copy(*q)){ #else if(*p != *q){ #endif break; } ++p; ++q; } out_relativePath.clear(); while(p != from.end()){ out_relativePath /= ".."; ++p; } while(q != to.end()){ out_relativePath /= *q++; } return true; } return false; } //#ifdef _WIN32 #if 0 /** \note This function cannot be used because the extension is omitted when the exploer hides the extension of registered file types. \todo Support the case where the exploer hides the extension of registered file types. */ const std::string toActualPathName(const std::string& path) { int codepage = _getmbcp(); size_t length = ::MultiByteToWideChar(codepage, 0, path.c_str(), path.size(), NULL, 0); if(length >= 0){ wchar_t wpath[MAX_PATH]; ::MultiByteToWideChar(codepage, 0, path.c_str(), path.size(), wpath, MAX_PATH); // The following code was based on the code posted at // http://stackoverflow.com/questions/74451/getting-actual-file-name-with-proper-casing-on-windowsis // Thank you. const wchar_t kSeparator = L'\\'; size_t i = 0; std::wstring result; // for network paths (\\server\share\RestOfPath), getting the display // name mangles it into unusable form (e.g. "\\server\share" turns // into "share on server (server)"). So detect this case and just skip // up to two path components if( length >= 2 && wpath[0] == kSeparator && wpath[1] == kSeparator ) { int skippedCount = 0; i = 2; // start after '\\' while( i < length && skippedCount < 2 ) { if( wpath[i] == kSeparator ){ ++skippedCount; } ++i; } result.append( wpath, i ); } // for drive names, just add it uppercased else if( length >= 2 && wpath[1] == L':' ) { result += towupper(wpath[0]); result += L':'; if( length >= 3 && wpath[2] == kSeparator ){ result += kSeparator; i = 3; // start after drive, colon and separator } else { i = 2; // start after drive and colon } } size_t lastComponentStart = i; bool addSeparator = false; while( i < length ) { // skip until path separator while( i < length && wpath[i] != kSeparator ) { ++i; } if( addSeparator ) { result += kSeparator; } // if we found path separator, get real filename of this // last path name component bool foundSeparator = (i < length); wpath[i] = 0; SHFILEINFOW info; // nuke the path separator so that we get real name of current path component info.szDisplayName[0] = 0; if( SHGetFileInfoW( wpath, 0, &info, sizeof(info), SHGFI_DISPLAYNAME ) ) { result += info.szDisplayName; } else { // most likely file does not exist. // So just append original path name component. result.append( wpath + lastComponentStart, i - lastComponentStart ); } // restore path separator that we might have nuked before if( foundSeparator ){ wpath[i] = kSeparator; } ++i; lastComponentStart = i; addSeparator = true; } length = ::WideCharToMultiByte(codepage, 0, &result[0], result.size(), NULL, 0, NULL, NULL); if(length >= 0){ std::vector converted(length + 1); ::WideCharToMultiByte(codepage, 0, &result[0], result.size(), &converted[0], length + 1, NULL, NULL); return std::string(&converted[0], length); } } return path; // failed } #endif } choreonoid-1.1.0+dfsg/src/Util/FileUtil.h000066400000000000000000000017031207742442300201610ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_FILE_UTIL_H_INCLUDED #define CNOID_UTIL_FILE_UTIL_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { CNOID_EXPORT void makePathCompact( const boost::filesystem::path& path, boost::filesystem::path& out_compact); CNOID_EXPORT bool findSubDirectory( const boost::filesystem::path& directory, const boost::filesystem::path& path, boost::filesystem::path& out_subdirectory); CNOID_EXPORT bool findRelativePath( const boost::filesystem::path& from, const boost::filesystem::path& to, boost::filesystem::path& out_relativePath); //#ifdef _WIN32 #if 0 CNOID_EXPORT const std::string toActualPathName(const std::string& pathName); #else inline const std::string& toActualPathName(const std::string& pathName) { return pathName; } #endif } #endif choreonoid-1.1.0+dfsg/src/Util/GaussianFilter.h000066400000000000000000000046651207742442300213760ustar00rootroot00000000000000/*! @file @brief Header file of gaussian filter functions @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_GAUSSIAN_FILTER_H_INCLUDED #define CNOID_UTIL_GAUSSIAN_FILTER_H_INCLUDED #include #include namespace cnoid { template void setGaussWindow(T sigma, int range, std::vector& out_window) { const double mu = 0.0; out_window.resize(range * 2 + 1); T x = 0.0; for(int i = 0; i <= range; ++i){ out_window[i + range] = exp(- (x - mu) * (x - mu) / (2.0 * sigma * sigma)); } // nomalization T area_half = out_window[range] / 2.0; for(int i=1; i <= range; ++i){ area_half += out_window[i + range]; } const T area = area_half * 2.0; out_window[range] /= area; for(int i=1; i <= range; ++i){ T& v = out_window[i + range]; v /= area; out_window[range - i] = v; } } template void applyGaussianFilter(RESULTVECTOR& result, const SRCVECTOR& src, std::vector& gwin, ELEMENT zero) { const int range = (gwin.size() - 1) / 2; const int size = src.size(); // head for(int i=0; i < range; i++){ ELEMENT v = zero; T ave = 0.0; for(int j = - i; j <= +range; j++){ v += src[i+j] * gwin[j+range]; ave += gwin[j+range]; } result[i] = v / ave; } // body for(int i=range; i < size - range; i++){ ELEMENT v = zero; for(int j=-range; j <= +range; j++){ v += src[i+j] * gwin[j+range]; } result[i] = v; } // tail for(int i = size - range; i < size; i++){ ELEMENT v = zero; T ave = 0.0; for(int j=-range; j < size - i; j++){ v += src[i+j] * gwin[j+range]; ave += gwin[j+range]; } result[i] = v / ave; } } template void applyGaussianFilter(RESULTVECTOR& result, const SRCVECTOR& src, T sigma, int range, ELEMENT zero) { std::vector gwin; setGaussWindow(sigma, range, gwin); applyGaussianFilter(result, src, gwin, zero); } } #endif choreonoid-1.1.0+dfsg/src/Util/ImageConverter.cpp000066400000000000000000000163071207742442300217170ustar00rootroot00000000000000/** @file ImageConverter.cpp @brief Implementation of Image Converter class @author K.FUKUDA @author Shin'ichiro Nakaoka */ #include "ImageConverter.h" #include #include extern "C" { #define XMD_H #include } #include using namespace std; using namespace cnoid; ImageConverter::ImageConverter(void) { image = new SFImage; }; ImageConverter::~ImageConverter(void) { delete image; }; /** @brief initialize "SFImage" @note use before using "SFImage" structure @return bool true : succeeded / false : failed */ bool ImageConverter::initializeSFImage( ) { image->width = 0; image->height = 0; image->numComponents = 0; image->pixels.clear(); return true; } /** @brief convert ImageTexture node to PixelTexture node @note read image data from VrmlImageTexture::url and store pixel data to VrmlPixelTexture::image. *.png and *.jpg are supported. Currentry, multi url is not supported. @return bool true : succeeded / false : failed */ SFImage* ImageConverter::convert(const std::string& url) { string ext = url.substr( url.rfind( '.' ) ); // convert the ext name into lower letters transform( ext.begin(), ext.end(), ext.begin(), (int(*)(int))tolower ); if( !ext.compare( ".png" ) ) { if(loadPNG( url )) return image; } else if( !ext.compare( ".jpg" ) ) { if(loadJPEG( url )) return image; } else { // cerr << "ImageTexture read error: unsupported format." << '\n'; } image->height = 0; image->width = 0; image->numComponents = 0; image->pixels.resize(0); return image; } /** @brief load PNG file @note load and fill VrmlPixelTexture::image from PNG. @return bool true : succeeded / false : failed */ bool ImageConverter::loadPNG(const std::string& filePath) { initializeSFImage( ); FILE* fp = 0; try { fp = fopen(filePath.c_str(), "rb"); if(!fp){ throw "File open error."; } png_size_t number = 8; png_byte header[8]; int is_png; size_t n = fread(header, 1, number, fp); if(n != number){ throw "File is not png."; } is_png = !png_sig_cmp(header, 0, number); if(!is_png){ throw "File is not png."; } png_structp pPng; pPng = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); if(!pPng){ throw "Failed to create png_struct"; } png_infop pInfo; pInfo = png_create_info_struct(pPng); if(!pInfo){ png_destroy_read_struct( &pPng, NULL, NULL ); throw "Failed to create png_info"; } png_init_io(pPng, fp); png_set_sig_bytes( pPng, (int)number ); png_read_info(pPng, pInfo); png_uint_32 width = png_get_image_width( pPng, pInfo ); png_uint_32 height = png_get_image_height( pPng, pInfo ); png_byte color_type = png_get_color_type( pPng, pInfo ); png_byte depth = png_get_bit_depth( pPng, pInfo ); if (png_get_valid( pPng, pInfo, PNG_INFO_tRNS)) png_set_tRNS_to_alpha(pPng); if (depth < 8) png_set_packing(pPng); int numComponents; switch( color_type ) { case PNG_COLOR_TYPE_GRAY: numComponents = 1; if(depth < 8) png_set_expand_gray_1_2_4_to_8(pPng); break; case PNG_COLOR_TYPE_GRAY_ALPHA: numComponents = 2; if (depth == 16) png_set_strip_16(pPng); break; case PNG_COLOR_TYPE_RGB: numComponents = 3; if (depth == 16) png_set_strip_16(pPng); break; case PNG_COLOR_TYPE_RGB_ALPHA: numComponents = 4; if (depth == 16) png_set_strip_16(pPng); break; case PNG_COLOR_TYPE_PALETTE: png_set_palette_to_rgb(pPng); numComponents = 3; break; default: numComponents = 1; // throw "Unsupported color type."; } png_read_update_info( pPng, pInfo ); unsigned char** row_pointers; row_pointers = (png_bytepp)malloc(height * sizeof(png_bytep)); png_uint_32 rowbytes = png_get_rowbytes(pPng, pInfo); image->pixels.resize(rowbytes*height); for(png_uint_32 i=0; i < height; i++){ row_pointers[i] = &(image->pixels[i*rowbytes]); } png_read_image(pPng, row_pointers); image->width = width; image->height = height; image->numComponents = numComponents; free(row_pointers); png_destroy_read_struct( &pPng, &pInfo, NULL ); fclose(fp); } catch( char * str ) { cerr << "PNG read error: " << str << '\n'; if(fp) fclose(fp); return false; } return true; } /** @brief load JPEG file @note load and fill VrmlPixelTexture::image from JPEG. @return bool true : succeeded / false : failed */ bool ImageConverter::loadJPEG(const std::string& filePath) { initializeSFImage( ); FILE* fp = 0; try { // File open fp = fopen(filePath.c_str(), "rb"); if( !fp ) throw "File open error."; struct jpeg_decompress_struct cinfo; struct jpeg_error_mgr jerr; // Step 1: allocate and initialize JPEG decompression object cinfo.err = jpeg_std_error( &jerr ); jpeg_create_decompress( &cinfo ); // Step 2: specify data source (eg, a file) jpeg_stdio_src( &cinfo, fp ); // Step 3: read file parameters with jpeg_read_header() (void)jpeg_read_header( &cinfo, TRUE ); // Step 4: set parameters for decompression // Step 5: Start decompression (void)jpeg_start_decompress( &cinfo ); // get image attribute image->width = cinfo.output_width; image->height = cinfo.output_height; image->numComponents = cinfo.out_color_components; JSAMPARRAY row_pointers; row_pointers = (JSAMPARRAY)malloc( sizeof( JSAMPROW ) * image->height ); image->pixels.resize(cinfo.output_components * image->width * image->height); for (int i = 0; i < image->height; i++ ) row_pointers[i] = &(image->pixels[i * cinfo.output_components * image->width]); while( cinfo.output_scanline < cinfo.output_height ) { jpeg_read_scanlines( &cinfo, row_pointers + cinfo.output_scanline, cinfo.output_height - cinfo.output_scanline ); } free(row_pointers); // Step 7: Finish decompression (void)jpeg_finish_decompress( &cinfo ); // Step 8: Release JPEG decompression object jpeg_destroy_decompress( &cinfo ); fclose( fp ); } catch( char * str ) { cerr << "JPEG read error: " << str << '\n'; if( fp ) fclose( fp ); return false; } return true; } choreonoid-1.1.0+dfsg/src/Util/ImageConverter.h000066400000000000000000000011531207742442300213550ustar00rootroot00000000000000/*! @file ImageConverter.h @brief Header file of Image Converter class @author K.FUKUDA */ #ifndef CNOID_UTIL_IMAGECONVERTER_H_INCLUDED #define CNOID_UTIL_IMAGECONVERTER_H_INCLUDED #include "VrmlNodes.h" #include "exportdecl.h" namespace cnoid { class ImageConverter { private: bool initializeSFImage(); bool loadPNG(const std::string & filePath); bool loadJPEG(const std::string & filePath); public: SFImage* image; ImageConverter(void); ~ImageConverter(void); CNOID_EXPORT SFImage* convert(const std::string& url); }; }; #endif choreonoid-1.1.0+dfsg/src/Util/Interpolator.cpp000066400000000000000000000125111207742442300214600ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "Interpolator.h" #include #include using namespace std; using namespace cnoid; void Interpolator::clear() { samples.clear(); prevReferredSegments = -1; } int Interpolator::numSamples() { return samples.size(); } int Interpolator::appendSample(double x, double y) { if(!samples.empty()){ Sample& prev = samples.back(); if(fabs(prev.x - x) < std::numeric_limits::epsilon()){ samples.pop_back(); } } Sample s; s.x = x; s.y = y; s.yp = 0.0; s.isEdge = false; s.isNaturalEdge = false; s.segmentType = UNDETERMINED; int index = samples.size(); samples.push_back(s); return index; } void Interpolator::setEndPoint(int sampleIndex, bool isNatural) { Sample& sample = samples[sampleIndex]; sample.isEdge = true; sample.isNaturalEdge = isNatural; sample.yp = 0.0; // first-order derivative (velocity) } bool Interpolator::update() { int n = samples.size(); int s = 0; while(true){ const int m = n - s; if(m < 2){ break; } if(m >= 3 && !samples[s + 1].isEdge){ s = updateCubicSplineSegment(s); } else { s = updateSegmentBetweenTwoSamples(s); } } return (n >= 2); } int Interpolator::updateCubicSplineSegment(int begin) { Sample& s0 = samples[begin]; s0.segmentType = CUBIC_SPLINE; s0.isEdge = true; if(s0.isNaturalEdge){ s0.a = 0.0; s0.b = 0.0; } else { Sample& s1 = samples[begin + 1]; s0.a = -0.5; s0.b = (3.0 / (s1.x - s0.x)) * ((s1.y - s0.y) / (s1.x - s0.x) - s0.yp); } const int n = samples.size(); int i = (begin + 1); while(true) { Sample& s0 = samples[i-1]; Sample& s1 = samples[i]; Sample& s2 = samples[i+1]; s1.segmentType = CUBIC_SPLINE; double sig = (s1.x - s0.x) / (s2.x - s0.x); double p = sig * s0.a + 2.0; s1.a = (sig - 1.0) / p; double b = (s2.y - s1.y) / (s2.x - s1.x) - (s1.y - s0.y) / (s1.x - s0.x); s1.b = (6.0 * b / (s2.x - s0.x) - sig * s0.b) / p; if(s2.isEdge || i == (n - 2)){ break; } ++i; } double qf; double bf; Sample& sf0 = samples[i]; Sample& sf = samples[i+1]; const int next = i + 1; sf.isEdge = true; if(sf.isNaturalEdge){ qf = 0.0; bf = 0.0; } else { qf = 0.5; bf = (3.0 / (sf.x - sf0.x)) * (sf.yp - (sf.y - sf0.y) / (sf.x - sf0.x)); } const double a_save = sf.a; sf.a = (bf - qf * sf0.b) / (qf * sf0.a + 1.0); while(i >= begin){ Sample& s0 = samples[i]; Sample& s1 = samples[i+1]; s0.a = s0.a * s1.a + s0.b; --i; } sf.a_end = sf.a; sf.a = a_save; return next; } int Interpolator::updateSegmentBetweenTwoSamples(int begin) { Sample& s0 = samples[begin]; s0.segmentType = POLYNOMINAL; s0.isEdge = true; Sample& s1 = samples[begin+1]; s1.isEdge = true; const double h = (s1.x - s0.x); const double h2 = h * h; const double h3 = h2 * h; const double d0 = s0.isEdge ? s0.yp : 0.0; const double d1 = s1.isEdge ? s1.yp : 0.0; s0.a = d0; s0.b = 3.0 * (s1.y - s0.y) / h2 - (2.0 * d0 - d1) / h; s0.c = (d0 + d1) / h2 + 2.0 * (s0.y - s1.y) / h3; return begin + 1; } void Interpolator::getDomain(double& out_lower, double& out_upper) { if(samples.empty()){ out_lower = 0.0; out_upper = 0.0; } else { out_lower = samples.front().x; out_upper = samples.back().x; } } double Interpolator::interpolate(double x) { int lower; int upper; const int n = samples.size(); int k; if(prevReferredSegments >= 0 && prevReferredSegments < (n - 1)){ k = prevReferredSegments; if(x >= samples[k].x && x < samples[k+1].x){ goto calc; } } lower = 0; upper = n - 1; if(x < samples[0].x){ return samples[0].y; } else if(x >= samples[upper].x){ return samples[upper].y; } while(upper - lower > 1){ k = (upper + lower) / 2; if(samples[k].x > x){ upper = k; } else { lower = k; } } k = lower; calc: prevReferredSegments = k; const Sample& s0 = samples[k]; const Sample& s1 = samples[k+1]; if(s0.segmentType == CUBIC_SPLINE){ const double a_end = (s1.isEdge ? s1.a_end : s1.a); const double h = s1.x - s0.x; const double A = (s1.x - x) / h; const double B = (x - s0.x) / h; return A*s0.y + B*s1.y + ((A*A*A - A) * s0.a + (B*B*B - B) * a_end) * (h*h) / 6.0; } else if(s0.segmentType == POLYNOMINAL){ const double& a0 = s0.y; const double& a1 = s0.a; const double& a2 = s0.b; const double& a3 = s0.c; const double h = x - s0.x; const double h2 = h * h; const double h3 = h2 * h; return (a0 + a1 * h + a2 * h2 + a3 * h3); } return 0.0; } /* bool interpolate(double x, double& out_y, double& out_d2, double& out_d3); bool interpolate(double xBegin, double step, double* out_ybuf); bool interpolate(double xBegin, double step, double* out_ybuf, double* out_d2buf, double* out_d3buf); */ choreonoid-1.1.0+dfsg/src/Util/Interpolator.h000066400000000000000000000026601207742442300211310ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_INTERPOLATOR_H_INCLUDED #define CNOID_UTIL_INTERPOLATOR_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT Interpolator { public: void clear(); int appendSample(double x, double y); void setEndPoint(int sampleIndex, bool isNatural = false); int numSamples(); bool update(); void getDomain(double& out_lower, double& out_upper); double interpolate(double x); //bool interpolate(double x, double& out_y, double& out_d2, double& out_d3); //bool interpolate(double xBegin, double step, double* out_ybuf); //bool interpolate(double xBegin, double step, double* out_ybuf, double* out_d2buf, double* out_d3buf); private: int updateCubicSplineSegment(int begin); int updateSegmentBetweenTwoSamples(int begin); enum SegmentType { UNDETERMINED, CUBIC_SPLINE, POLYNOMINAL }; struct Sample { double x; double y; double yp; // coefficients double a; double a_end; double b; double c; bool isEdge; bool isNaturalEdge; SegmentType segmentType; }; std::vector samples; int prevReferredSegments; }; } #endif choreonoid-1.1.0+dfsg/src/Util/MultiAffine3Seq.cpp000066400000000000000000000121741207742442300217420ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "MultiAffine3Seq.h" #include "PlainSeqFormatLoader.h" #include "YamlNodes.h" #include "YamlWriter.h" #include "EigenUtil.h" #include using namespace std; using namespace boost; using namespace cnoid; MultiAffine3Seq::MultiAffine3Seq(int numParts, int numFrames, double frameRate) : MultiAffine3Seq::BaseSeqType("MultiAffine3Seq", numParts, numFrames, frameRate) { } MultiAffine3Seq::MultiAffine3Seq(const MultiAffine3Seq& org) : MultiAffine3Seq::BaseSeqType(org) { } MultiAffine3Seq::~MultiAffine3Seq() { } bool MultiAffine3Seq::loadPlainFormat(const std::string& filename) { PlainSeqFileLoader loader; if(!loader.load(filename)){ setIoErrorMessage(loader.errorMessage()); return false; } if(loader.numParts() < 12){ setIoErrorMessage(filename + "does not have 12-columns " "(3 for position vectors, 9 for attitde matrices)"); return false; } setDimension(loader.numFrames(), 1); setTimeStep(loader.timeStep()); int i = 0; View base = part(0); for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){ vector& data = *it; base[i].translation() << data[1], data[2], data[3]; base[i].linear() << data[ 4], data[ 5], data[ 6], data[ 7], data[ 8], data[ 9], data[10], data[11], data[12]; ++i; } return true; } bool MultiAffine3Seq::saveTopPartAsPlainFormat(const std::string& filename) { format f("%1$.4f"); const int nFrames = numFrames(); if(nFrames > 0 && numParts() > 0){ ofstream os(filename.c_str()); if(!os){ setIoErrorMessage(filename + " cannot be opened."); return false; } const double r = frameRate(); View base = part(0); for(int i=0; i < nFrames; ++i){ os << (f % (i / r)); const Affine3& T = base[i]; for(int j=0; j < 3; ++j){ os << " " << T.translation()[j]; } for(int j=0; j < 3; ++j){ for(int k=0; k < 3; ++k){ double m = T.linear()(j, k); if(fabs(m) < 1.0e-14){ m = 0.0; } os << " " << m; } } os << " 0 0 0 0 0 0"; // velocity elements (dv, omega) os << "\n"; } return true; } return false; } static inline void writeAffine3(YamlWriter& writer, const Affine3& value) { writer.startFlowStyleSequence(); writer.putScalar(value.translation().x()); writer.putScalar(value.translation().y()); writer.putScalar(value.translation().z()); Vector3 rpy(rpyFromRot(value.linear())); writer.putScalar(rpy[0]); writer.putScalar(rpy[1]); writer.putScalar(rpy[2]); writer.endSequence(); } bool MultiAffine3Seq::write(YamlWriter& writer) { bool result = false; writer.startMapping(); if(BaseSeqType::write(writer)){ writer.putKeyValue("format", "XYZRPY"); writer.putKey("frames"); writer.startSequence(); const int m = numParts(); const int n = numFrames(); for(int i=0; i < n; ++i){ View f = frame(i); writer.startFlowStyleSequence(); for(int j=0; j < m; ++j){ writeAffine3(writer, f[j]); } writer.endSequence(); } writer.endSequence(); result = true; } writer.endMapping(); return result; } static void readAffine3(const YamlSequence& node, Affine3& out_value) { if(node.size() == 6){ Affine3::TranslationPart t = out_value.translation(); t[0] = node[0].toDouble(); t[1] = node[1].toDouble(); t[2] = node[2].toDouble(); const double r = node[3].toDouble(); const double p = node[4].toDouble(); const double y = node[5].toDouble(); out_value.linear() = rotFromRpy(r, p, y); } } bool MultiAffine3Seq::read(const YamlMapping& archive) { bool loaded = false; if(BaseSeqType::read(archive)){ const string& type = archive["type"].toString(); if((type == seqType() || type == "MultiSe3Seq") && (archive["format"].toString() == "XYZRPY")){ const YamlSequence& values = *archive.findSequence("frames"); if(values.isValid()){ const int nParts = archive["numParts"].toInt(); const int nFrames = values.size(); setDimension(nFrames, nParts); for(int i=0; i < nFrames; ++i){ const YamlSequence& frameNode = *values[i].toSequence(); View f = frame(i); const int n = std::min(frameNode.size(), nParts); for(int j=0; j < n; ++j){ readAffine3(*frameNode[j].toSequence(), f[j]); } } loaded = true; } } } return loaded; } choreonoid-1.1.0+dfsg/src/Util/MultiAffine3Seq.h000066400000000000000000000022121207742442300213770ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_MULTI_AFFINE3_SEQ_H_INCLUDED #define CNOID_UTIL_MULTI_AFFINE3_SEQ_H_INCLUDED #include "MultiSeq.h" #include "EigenTypes.h" #include "exportdecl.h" namespace cnoid { class YamlWriter; class YamlMapping; class CNOID_EXPORT MultiAffine3Seq : public MultiSeq > { typedef MultiSeq > BaseSeqType; public: typedef boost::shared_ptr Ptr; MultiAffine3Seq(int numParts = 1, int numFrames = 0, double frameRate = DEFAULT_FRAME_RATE); MultiAffine3Seq(const MultiAffine3Seq& org); virtual ~MultiAffine3Seq(); virtual bool write(YamlWriter& writer); virtual bool read(const YamlMapping& archive); virtual bool loadPlainFormat(const std::string& filename); bool saveTopPartAsPlainFormat(const std::string& filename); protected: virtual Affine3 defaultValue() const { return Affine3::Identity(); } }; typedef MultiAffine3Seq::Ptr MultiAffine3SeqPtr; } #endif choreonoid-1.1.0+dfsg/src/Util/MultiSeq.h000066400000000000000000000130561207742442300202130ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_MULTI_SEQ_H_INCLUDED #define CNOID_UTIL_MULTI_SEQ_H_INCLUDED #include "SeqBase.h" #include #include namespace cnoid { template > class MultiSeq : public MultiSeqBase { public: typedef boost::shared_ptr< MultiSeq > Ptr; typedef typename boost::multi_array Container; typedef typename Container::index_range IndexRange; typedef typename Container::template array_view<1>::type View; MultiSeq(const char* seqType, int numParts = 1, int numFrames = 0.0, double frameRate = 100.0) : MultiSeqBase(seqType), container(boost::extents[numFrames][numParts]) { frameRate_ = frameRate; numFrames_ = numFrames; } MultiSeq(const MultiSeq& org) : MultiSeqBase(org), container(org.container) { frameRate_ = org.frameRate_; numFrames_ = org.numFrames_; } virtual ~MultiSeq() { } virtual void setDimension(int newNumFrames, int newNumParts, bool clearNewElements = false) { const int prevNumParts = numParts(); const int prevNumFrames = numFrames(); int reservedNumFrames = container.size(); if(newNumFrames > reservedNumFrames){ container.resize(boost::extents[newNumFrames][newNumParts]); } else { container.resize(boost::extents[reservedNumFrames][newNumParts]); } numFrames_ = newNumFrames; if(clearNewElements){ int i = (prevNumFrames == 0) ? 0 : prevNumParts; while(i < newNumParts){ View frames = part(i++); std::fill(frames.begin(), frames.end(), defaultValue()); } if(prevNumFrames > 0 && (numFrames_ > prevNumFrames)){ for(int i=0; i < prevNumParts; ++i){ View frames = part(i); const ElementType& last = frames[prevNumFrames - 1]; std::fill(frames.begin() + prevNumFrames, frames.end(), last); } } } } virtual double getFrameRate() const { return frameRate_; } inline double frameRate() const { return frameRate_; } virtual void setFrameRate(double frameRate) { frameRate_ = frameRate; } virtual void setNumParts(int newNumParts, bool clearNewElements = false) { setDimension(numFrames(), newNumParts, clearNewElements); } virtual int getNumFrames() const { return numFrames_; } inline int numFrames() const { return numFrames_; } virtual void setNumFrames(int newNumFrames, bool clearNewElements = false) { setDimension(newNumFrames, numParts(), clearNewElements); } virtual int getNumParts() const { return container.shape()[1]; } inline int numParts() const { return container.shape()[1]; } /** @if jp シーケンスã®æ™‚é–“é•·ã‚’è¿”ã™ã€‚ @note ã“ã®æ™‚é–“ *未満* ã®æ™‚é–“ã«ã¤ã„ã¦ã¯æœ‰åŠ¹ãªãƒ‡ãƒ¼ã‚¿ãŒå­˜åœ¨ã™ã‚‹ã€‚ ã“ã®æ™‚é–“ã®ãƒ‡ãƒ¼ã‚¿ã¯å­˜åœ¨ã—ãªã„ã®ã§ã€ãã®ã‚ˆã†ãªã‚¢ã‚¯ã‚»ã‚¹ã—ã¦ã¯ã„ã‘ãªã„。 @endif */ inline double timeLength() const { return numFrames_ / frameRate(); } inline int frameOfTime(double time) const { return (int)(time * frameRate_); } inline double timeOfFrame(int frame) const { return (frame / frameRate_); } inline const ElementType& at(int frame, int part) const { return container[frame][part]; } inline ElementType& at(int frame, int part) { return container[frame][part]; } inline View part(int index) { return container[boost::indices[IndexRange(0, numFrames())][index]]; } inline View frame(int index) { return container[index][boost::indices[IndexRange(0, numParts())]]; } /* Container::reference operator[](index i) { return container[i]; } Container::const_reference operator[](index i) const { return container[i]; } */ inline View appendFrame() { const size_t n = numFrames_; if(n >= container.size()){ setNumFrames(n * 2); } numFrames_ = n + 1; return frame(n); } template inline void appendFrame(const ArrayType& v) { View frame = appendFrame(); const int m = numParts(); for(int i=0; i < m; ++i){ frame[i] = v[i]; } } virtual bool read(const YamlMapping& archive) { return MultiSeqBase::read(archive); } virtual bool write(YamlWriter& writer) { return MultiSeqBase::write(writer); } protected: Container container; int numFrames_; double frameRate_; virtual ElementType defaultValue() const { return ElementType(); } }; } #endif choreonoid-1.1.0+dfsg/src/Util/MultiValueSeq.cpp000066400000000000000000000056001207742442300215370ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "MultiValueSeq.h" #include "PlainSeqFormatLoader.h" #include "YamlNodes.h" #include "YamlWriter.h" using namespace std; using namespace cnoid; MultiValueSeq::MultiValueSeq(int numParts, int numFrames, double frameRate) : BaseSeqType("MultiValueSeq", numParts, numFrames, frameRate) { } MultiValueSeq::MultiValueSeq(const MultiValueSeq& org) : BaseSeqType(org) { } MultiValueSeq::~MultiValueSeq() { } bool MultiValueSeq::loadPlainFormat(const std::string& filename) { PlainSeqFileLoader loader; if(!loader.load(filename)){ setIoErrorMessage(loader.errorMessage()); return false; } setDimension(loader.numFrames(), loader.numParts()); setFrameRate(1.0 / loader.timeStep()); int i = 0; for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){ copy((it->begin() + 1), it->end(), frame(i++).begin()); } return true; } bool MultiValueSeq::saveAsPlainFormat(const std::string& filename) { ofstream os(filename.c_str()); os.setf(ios::fixed); if(!os){ setIoErrorMessage(filename + " cannot be opened."); return false; } const int n = numFrames(); const int m = numParts(); const double r = frameRate(); for(int i=0; i < n; ++i){ os << (i / r); View v = frame(i); for(int j=0; j < m; ++j){ os << " " << v[j]; } os << "\n"; } return true; } bool MultiValueSeq::write(YamlWriter& writer) { bool result = false; writer.startMapping(); if(BaseSeqType::write(writer)){ writer.putKey("frames"); writer.startSequence(); const int n = numFrames(); const int m = numParts(); for(int i=0; i < n; ++i){ writer.startFlowStyleSequence(); View v = frame(i); for(int j=0; j < m; ++j){ writer.putScalar(v[j]); } writer.endSequence(); } writer.endSequence(); result = true; } writer.endMapping(); return result; } bool MultiValueSeq::read(const YamlMapping& archive) { if(BaseSeqType::read(archive) && (archive["type"].toString() == seqType())){ const YamlSequence& values = *archive.findSequence("frames"); if(values.isValid()){ const int nParts = archive["numParts"].toInt(); const int nFrames = values.size(); setDimension(nFrames, nParts); for(int i=0; i < nFrames; ++i){ const YamlSequence& frameNode = *values[i].toSequence(); const int n = std::min(frameNode.size(), nParts); View v = frame(i); for(int j=0; j < n; ++j){ v[j] = frameNode[j].toDouble(); } } return true; } } return false; } choreonoid-1.1.0+dfsg/src/Util/MultiValueSeq.h000066400000000000000000000016141207742442300212050ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_MULTI_VALUE_SEQ_H_INCLUDED #define CNOID_UTIL_MULTI_VALUE_SEQ_H_INCLUDED #include "MultiSeq.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT MultiValueSeq : public MultiSeq { typedef MultiSeq BaseSeqType; public: typedef boost::shared_ptr Ptr; MultiValueSeq(int numParts = 1, int numFrames = 0, double frameRate = 100.0); MultiValueSeq(const MultiValueSeq& org); virtual ~MultiValueSeq(); virtual bool write(YamlWriter& writer); virtual bool read(const YamlMapping& archive); virtual bool loadPlainFormat(const std::string& filename); virtual bool saveAsPlainFormat(const std::string& filename); }; typedef MultiValueSeq::Ptr MultiValueSeqPtr; } #endif choreonoid-1.1.0+dfsg/src/Util/NullOut.cpp000066400000000000000000000010711207742442300203770ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "NullOut.h" #include #include using namespace std; using namespace boost; namespace { class NullSink : public iostreams::sink { public: streamsize write(const char* s, streamsize n) { return n; } }; } namespace cnoid { std::ostream& nullout() { static NullSink nullSink; static iostreams::stream_buffer sbuf(nullSink); static ostream os(&sbuf); return os; } } choreonoid-1.1.0+dfsg/src/Util/NullOut.h000066400000000000000000000003521207742442300200450ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_NULL_OUT_H_INCLUDED #define CNOID_UTIL_NULL_OUT_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { CNOID_EXPORT std::ostream& nullout(); } #endif choreonoid-1.1.0+dfsg/src/Util/PlainSeqFormatLoader.cpp000066400000000000000000000034741207742442300230220ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "PlainSeqFormatLoader.h" #include #include #include using namespace std; using namespace boost; using namespace cnoid; bool PlainSeqFileLoader::load(const std::string& filename) { ifstream is(filename.c_str()); string errorMessageBase("\""); errorMessageBase += filename + "\""; if(!is){ errorMessage_ = errorMessageBase + " cannot be opened."; return false; } size_t nColumns = 0; size_t nLines = 0; typedef char_separator Separator; typedef tokenizer Tokenizer; Separator sep(" \t\r\n"); seq.clear(); string line; while(getline(is, line)){ nLines++; seq.push_back(vector(nColumns)); vector& v = seq.back(); v.clear(); Tokenizer tokens(line, sep); for(Tokenizer::iterator it = tokens.begin(); it != tokens.end(); ++it){ v.push_back(lexical_cast(*it)); } if(nColumns == 0){ nColumns = v.size(); } else if(v.size() != nColumns){ errorMessage_ = errorMessageBase + " contains different size columns"; return false; } } if(nColumns < 2 || nLines < 2){ errorMessage_ = errorMessageBase + ": Empty sequence."; return false; } numParts_ = nColumns - 1; numFrames_ = nLines; iterator it = seq.begin(); double time0 = (*it)[0]; double time1 = (*(++it))[0]; timeStep_ = time1 - time0; if(timeStep_ <= 0.0){ errorMessage_ = errorMessageBase + ": Time values are not arranged."; return false; } return true; } const std::string& PlainSeqFileLoader::errorMessage() { return errorMessage_; } choreonoid-1.1.0+dfsg/src/Util/PlainSeqFormatLoader.h000066400000000000000000000017211207742442300224600ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_PLAIN_SEQ_FILE_LOADER_H_INCLUDED #define CNOID_UTIL_PLAIN_SEQ_FILE_LOADER_H_INCLUDED #include #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT PlainSeqFileLoader { public: typedef std::list< std::vector > Seq; typedef Seq::iterator iterator; bool load(const std::string& filename); inline iterator begin() { return seq.begin(); } inline iterator end() { return seq.end(); } inline int numParts() { return numParts_; } inline int numFrames() { return numFrames_; } inline double timeStep() { return timeStep_; } const std::string& errorMessage(); private: Seq seq; int numParts_; int numFrames_; double timeStep_; std::string errorMessage_; }; } #endif choreonoid-1.1.0+dfsg/src/Util/RangeLimiter.cpp000066400000000000000000000014131207742442300213570ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "RangeLimiter.h" using namespace cnoid; void RangeLimiter::calcCrossPoint(RangeLimiter::TSegment* seg) { seg->uc = (uL - r * seg->upper) / (1.0 - r); seg->lc = (lL - r * seg->lower) / (1.0 - r); if(seg->lc > seg->uc){ seg->uc = seg->lc = (seg->upper * lL - seg->lower * uL) / (seg->upper + lL - uL - seg->lower); } } RangeLimiter::TSegment* RangeLimiter::mergeSegment(RangeLimiter::TSegment* seg, RangeLimiter::TSegment* prevSeg) { if(seg->upper > prevSeg->upper){ prevSeg->upper = seg->upper; } if(seg->lower < prevSeg->lower){ prevSeg->lower = seg->lower; } calcCrossPoint(prevSeg); segments.pop_back(); return &segments.back(); } choreonoid-1.1.0+dfsg/src/Util/RangeLimiter.h000066400000000000000000000135021207742442300210260ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_RANGE_LIMITER_H_INCLUDED #define CNOID_UTIL_RANGE_LIMITER_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class RangeLimiter { public: template bool apply(TArray& valueSeq, double upperLimit, double lowerLimit, double limitGrad, double edgeGradRatio = 0.5); private: double uL; double lL; double r; struct TSegment { int begin; int end; double upper; double lower; double uc; double lc; bool isOver; }; std::vector segments; CNOID_EXPORT void calcCrossPoint(TSegment* seg); CNOID_EXPORT TSegment* mergeSegment(TSegment* seg, TSegment* prevSeg); template TSegment* finalizeSegment(TSegment* seg, int endFrame, TArray& valueSeq); }; template RangeLimiter::TSegment* RangeLimiter::finalizeSegment (RangeLimiter::TSegment* seg, int endFrame, TArray& valueSeq) { seg->end = endFrame; int backFrame = seg->begin - 1; TSegment* prevSeg = &segments[segments.size() - 2]; while(backFrame >= 0){ if(backFrame < prevSeg->end){ return mergeSegment(seg, prevSeg); } double v = valueSeq[backFrame]; backFrame--; if((v < seg->uc) && (v > seg->lc)){ break; } } backFrame++; seg->begin = backFrame; return seg; } template bool RangeLimiter::apply (TArray& valueSeq, double upperLimit, double lowerLimit, double limitGrad, double edgeGradRatio) { r = limitGrad; uL = upperLimit; lL = lowerLimit; segments.clear(); TSegment dummy; dummy.isOver = false; dummy.end = -1; segments.push_back(dummy); segments.push_back(TSegment()); TSegment* seg = &segments.back(); seg->begin = 0; seg->upper = uL; seg->lower = lL; seg->isOver = false; calcCrossPoint(seg); int frame = 0; int n = valueSeq.size(); while(frame < n){ double v = valueSeq[frame]; bool isOver = false; if(v > seg->upper){ isOver = true; seg->upper = v; } else if(v < seg->lower){ isOver = true; seg->lower = v; } if(isOver){ calcCrossPoint(seg); if(!seg->isOver){ seg->isOver = true; seg->begin = frame; } } else if(seg->isOver) { if((v < seg->uc) && (v > seg->lc)){ TSegment* finalized = finalizeSegment(seg, frame, valueSeq); if(finalized != seg){ seg = finalized; continue; } else { TSegment nextSeg; nextSeg.begin = finalized->end; nextSeg.upper = uL; nextSeg.lower = lL; nextSeg.isOver = false; calcCrossPoint(&nextSeg); segments.push_back(nextSeg); seg = &segments.back(); } } } frame++; } if(seg->isOver){ while(true) { TSegment* finalized = finalizeSegment(seg, frame, valueSeq); if(finalized == seg){ break; } seg = finalized; } } else { segments.pop_back(); } for(std::size_t i=1; i < segments.size(); i++){ TSegment& seg = segments[i]; if(seg.isOver){ double ua, ub; if(seg.upper == uL){ ua = 0.0; ub = 0.0; } else { double k = ((uL - seg.uc) / (seg.upper - seg.uc)) * edgeGradRatio; double u = seg.upper - seg.uc; double l = uL - seg.uc; ub = ((1.0 - k) * u - 3.0 * (u - l)) / (u * u); ua = (-2.0 * u * ub - 1.0 + k) / (3.0 * u * u); } double la, lb; if(seg.lower == lL){ la = 0.0; lb = 0.0; } else { double k = ((lL - seg.lc) / (seg.lower - seg.lc)) * edgeGradRatio; double u = seg.lower - seg.lc; double l = lL - seg.lc; lb = ((1.0 - k) * u - 3.0 * (u - l)) / (u * u); la = (-2.0 * u * lb - 1.0 + k) / (3.0 * u * u); } for(int frame=seg.begin; frame < seg.end; frame++){ double v = valueSeq[frame]; if(v >= seg.uc){ double x = v - seg.uc; valueSeq[frame] = ua * x*x*x + ub * x*x + x + seg.uc; } else if(v <= seg.lc){ double x = v - seg.lc; valueSeq[frame] = la * x*x*x + lb * x*x + x + seg.lc; } } } } return true; } } #endif choreonoid-1.1.0+dfsg/src/Util/Referenced.h000066400000000000000000000021041207742442300205020ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_REFERENCED_H_INCLUDED #define CNOID_UTIL_REFERENCED_H_INCLUDED #include namespace cnoid { class Referenced; } namespace boost{ void intrusive_ptr_add_ref(cnoid::Referenced* obj); void intrusive_ptr_release(cnoid::Referenced* obj); } namespace cnoid { class Referenced { public: inline Referenced() { refCounter_ = 0; } virtual ~Referenced() { } protected: inline int refCounter() { return refCounter_; } private: friend void boost::intrusive_ptr_add_ref(Referenced* obj); friend void boost::intrusive_ptr_release(Referenced* obj); int refCounter_; }; typedef boost::intrusive_ptr ReferencedPtr; } namespace boost { inline void intrusive_ptr_add_ref(cnoid::Referenced* obj){ obj->refCounter_++; } inline void intrusive_ptr_release(cnoid::Referenced* obj){ obj->refCounter_--; if(obj->refCounter_ == 0){ delete obj; } } }; #endif choreonoid-1.1.0+dfsg/src/Util/Seq.h000066400000000000000000000047341207742442300172030ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_SEQ_H_INCLUDED #define CNOID_UTIL_SEQ_H_INCLUDED #include "SeqBase.h" #include #include "exportdecl.h" namespace cnoid { template class Seq : public SeqBase { public: typedef boost::shared_ptr< Seq > Ptr; Seq(const char* seqType, int nFrames = 0.0, double frameRate = 100.0) : SeqBase(seqType), container(nFrames) { frameRate_ = frameRate; } Seq(const Seq& org) : SeqBase(org), container(org.container) { frameRate_ = org.frameRate_; } virtual ~Seq() { } virtual double getFrameRate() const { return frameRate_; } inline double frameRate() const { return frameRate_; } virtual void setFrameRate(double frameRate) { frameRate_ = frameRate; } virtual int getNumFrames() const { return container.size(); } inline int numFrames() const { return container.size(); } virtual void setNumFrames(int n, bool clearNewElements = false) { if(clearNewElements){ container.resize(n, defaultValue()); } else { container.resize(n); } } inline bool empty() const { return container.empty(); } inline int frameOfTime(double time) const { return (int)(time * frameRate_); } inline double timeOfFrame(int frame) const { return (frame / frameRate_); } inline ElementType& operator[](int frameIndex) { return container[frameIndex]; } inline const ElementType& operator[](int frameIndex) const { return container[frameIndex]; } inline ElementType& at(int frameIndex) { return container[frameIndex]; } inline const ElementType& at(int frameIndex) const { return container[frameIndex]; } virtual bool read(const YamlMapping& archive) { return SeqBase::read(archive); } virtual bool write(YamlWriter& writer) { return SeqBase::write(writer); } protected: std::vector container; double frameRate_; virtual ElementType defaultValue() const { return ElementType(); } }; } #endif choreonoid-1.1.0+dfsg/src/Util/SeqBase.cpp000066400000000000000000000026131207742442300203230ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "SeqBase.h" #include "YamlNodes.h" #include "YamlWriter.h" using namespace std; using namespace cnoid; SeqBase::SeqBase(const char* seqType) : seqType_(seqType) { } SeqBase::SeqBase(const SeqBase& org) : seqType_(org.seqType_), purpose_(org.purpose_) { } SeqBase::~SeqBase() { } bool SeqBase::read(const YamlMapping& archive) { setPurpose(archive["purpose"].toString()); setFrameRate(archive["frameRate"].toDouble()); return true; } bool SeqBase::write(YamlWriter& writer) { if(!seqType().empty()){ writer.putKeyValue("type", seqType()); writer.putKeyValue("purpose", purpose()); writer.putKeyValue("frameRate", getFrameRate()); writer.putKeyValue("numFrames", getNumFrames()); return true; } return true; } bool MultiSeqBase::read(const YamlMapping& archive) { setPurpose(archive["purpose"].toString()); setFrameRate(archive["frameRate"].toDouble()); return true; } bool MultiSeqBase::write(YamlWriter& writer) { if(!seqType().empty()){ writer.putKeyValue("type", seqType()); writer.putKeyValue("purpose", purpose()); writer.putKeyValue("frameRate", getFrameRate()); writer.putKeyValue("numParts", getNumParts()); writer.putKeyValue("numFrames", getNumFrames()); return true; } return true; } choreonoid-1.1.0+dfsg/src/Util/SeqBase.h000066400000000000000000000057321207742442300177750ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_SEQ_BASE_H_INCLUDED #define CNOID_UTIL_SEQ_BASE_H_INCLUDED #include #include #include "exportdecl.h" namespace cnoid { class YamlMapping; class YamlWriter; static const double DEFAULT_FRAME_RATE = 100.0; class CNOID_EXPORT SeqBase { public: SeqBase(const char* seqType); SeqBase(const SeqBase& org); virtual ~SeqBase(); inline const std::string& seqType() const { return seqType_; } virtual double getFrameRate() const = 0; virtual void setFrameRate(double frameRate) = 0; inline double getTimeStep() const { return 1.0 / getFrameRate(); } inline void setTimeStep(double timeStep){ return setFrameRate(1.0 / timeStep); } virtual int getNumFrames() const = 0; virtual void setNumFrames(int n, bool clearNewElements = false) = 0; inline void setTimeLength(double length, bool clearNewElements = false){ return setNumFrames(static_cast(length * getFrameRate()), clearNewElements); } /** @if jp シーケンスã®æ™‚é–“é•·ã‚’è¿”ã™ã€‚ @note ã“ã®æ™‚é–“ *未満* ã®æ™‚é–“ã«ã¤ã„ã¦ã¯æœ‰åŠ¹ãªãƒ‡ãƒ¼ã‚¿ãŒå­˜åœ¨ã™ã‚‹ã€‚ ã“ã®æ™‚é–“ã®ãƒ‡ãƒ¼ã‚¿ã¯å­˜åœ¨ã—ãªã„ã®ã§ã€ãã®ã‚ˆã†ãªã‚¢ã‚¯ã‚»ã‚¹ã—ã¦ã¯ã„ã‘ãªã„。 @endif */ inline double getTimeLength() const { return getNumFrames() / getFrameRate(); } inline const std::string& purpose() { return purpose_; } virtual void setPurpose(const std::string& purpose) { purpose_ = purpose; } virtual bool read(const YamlMapping& archive); virtual bool write(YamlWriter& writer); inline const std::string& ioErrorMessage() const { return ioErrorMessage_; } protected: void setIoErrorMessage(const std::string& message) { ioErrorMessage_ = message; } private: const std::string seqType_; std::string purpose_; std::string ioErrorMessage_; }; typedef boost::shared_ptr SeqBasePtr; class CNOID_EXPORT MultiSeqBase : public SeqBase { public: MultiSeqBase(const char* seqType) : SeqBase(seqType) { } MultiSeqBase(const SeqBase& org) : SeqBase(org) { } virtual ~MultiSeqBase() { } virtual void setDimension(int numFrames, int numParts, bool claerNewElements = false) = 0; virtual void setNumParts(int numParts, bool clearNewElements = false) = 0; virtual int getNumParts() const = 0; virtual bool read(const YamlMapping& archive); virtual bool write(YamlWriter& writer); }; typedef boost::shared_ptr MultiSeqBasePtr; } #endif choreonoid-1.1.0+dfsg/src/Util/Sleep.h000066400000000000000000000007351207742442300175200ustar00rootroot00000000000000 #ifndef CNOID_UTIL_TIME_MEASURE_H_INCLUDED #define CNOID_UTIL_TIME_MEASURE_H_INCLUDED #ifdef WIN32 #include namespace cnoid { inline void msleep(int msec){ ::Sleep(msec); } inline void usleep(int usec){ ::Sleep(usec / 1000); } } #else #include namespace cnoid { inline void msleep(int msec){ ::usleep(msec * 1000); } inline void usleep(int usec){ ::usleep(usec); } } #endif #endif choreonoid-1.1.0+dfsg/src/Util/TimeMeasure.h000066400000000000000000000041621207742442300206660ustar00rootroot00000000000000 #ifndef CNOID_UTIL_TIME_MEASURE_H_INCLUDED #define CNOID_UTIL_TIME_MEASURE_H_INCLUDED #ifndef __WIN32__ #include namespace cnoid { class TimeMeasure { struct timeval tv; double time_; double totalTime_; int numCalls; public: inline TimeMeasure() { totalTime_ = 0.0; numCalls = 0; } inline void begin() { gettimeofday(&tv, 0); } inline void end(){ double beginTime = tv.tv_sec + (double)tv.tv_usec * 1.0e-6; gettimeofday(&tv, 0); double endTime = tv.tv_sec + (double)tv.tv_usec * 1.0e-6; time_ = endTime - beginTime; totalTime_ += time_; numCalls++; } inline double time() { return time_; } inline double totalTime() { return totalTime_; } inline double avarageTime() { return totalTime_ / numCalls; } }; #else #include typedef unsigned __int64 ulonglong; class TimeMeasure { ulonglong iTimerScale; ulonglong beginTime; ulonglong endTime; double time_; double totalTime_; int numCalls; public: inline TimeMeasure() { totalTime_ = 0.0; numCalls = 0; BOOL iDummyBool = QueryPerformanceFrequency ((LARGE_INTEGER *) &iTimerScale); if(!iDummyBool) iTimerScale=1; } inline void begin() { BOOL iDummyBool = QueryPerformanceCounter ((LARGE_INTEGER *) &beginTime); if(!iDummyBool) beginTime=1; } inline void end(){ BOOL iDummyBool = QueryPerformanceCounter ((LARGE_INTEGER *) &endTime); if(!iDummyBool) endTime=0; time_ = (double)(endTime - beginTime) / iTimerScale; totalTime_ += time_; numCalls++; } inline double time() { return time_; } inline double totalTime() { return totalTime_; } inline double avarageTime() { return totalTime_ / numCalls; } }; #endif #endif choreonoid-1.1.0+dfsg/src/Util/TriangleMeshShaper.cpp000066400000000000000000001605421207742442300225330ustar00rootroot00000000000000/*! @file TriangleMeshShaper.cpp @author Shin'ichiro Nakaoka */ #include "TriangleMeshShaper.h" #include "Triangulator.h" #include "EigenUtil.h" #include #include using namespace std; using namespace boost; using namespace cnoid; namespace cnoid { class TMSImpl { public: TMSImpl(TriangleMeshShaper* self); TriangleMeshShaper* self; int divisionNumber; bool isNormalGenerationMode; typedef map ShapeToGeometryMap; ShapeToGeometryMap shapeToOriginalGeometryMap; // for triangulation Triangulator triangulator; std::vector polygon; std::vector indexPositionMap; std::vector faceIndexMap; // for normal generation std::vector faceNormals; std::vector< vector > vertexIndexToFaceIndicesMap; std::vector< vector > vertexIndexToNormalIndicesMap; enum RemapType { REMAP_COLOR, REMAP_NORMAL }; SFNode getOriginalGeometry(VrmlShapePtr shapeNode); bool traverseShapeNodes(VrmlNode* node, AbstractVrmlGroup* parentNode, int indexInParent); bool convertShapeNode(VrmlShape* shapeNode); bool convertIndexedFaceSet(VrmlIndexedFaceSet* faceSet); template bool remapDirectMapObjectsPerFaces(TArray& objects, const char* objectName); bool checkAndRemapIndices(RemapType type, int numElements, MFInt32& indices, bool perVertex, VrmlIndexedFaceSet* triangleMesh); void putError1(const char* valueName); bool convertBox(VrmlBox* box, VrmlIndexedFaceSetPtr& triangleMesh); bool convertCone(VrmlCone* cone, VrmlIndexedFaceSetPtr& triangleMesh); bool convertCylinder(VrmlCylinder* cylinder, VrmlIndexedFaceSetPtr& triangleMesh); bool convertSphere(VrmlSphere* sphere, VrmlIndexedFaceSetPtr& triangleMesh); bool convertElevationGrid(VrmlElevationGrid* grid, VrmlIndexedFaceSetPtr& triangleMesh); bool convertExtrusion(VrmlExtrusion* extrusion, VrmlIndexedFaceSetPtr& triangleMesh); void generateNormals(VrmlIndexedFaceSetPtr& triangleMesh); void calculateFaceNormals(VrmlIndexedFaceSetPtr& triangleMesh); void setVertexNormals(VrmlIndexedFaceSetPtr& triangleMesh); void setFaceNormals(VrmlIndexedFaceSetPtr& triangleMesh); bool setTexCoordIndex(VrmlIndexedFaceSetPtr faseSet ); void putMessage(const std::string& message); }; } TriangleMeshShaper::TriangleMeshShaper() { impl = new TMSImpl(this); } TMSImpl::TMSImpl(TriangleMeshShaper* self) : self(self) { divisionNumber = 20; isNormalGenerationMode = true; } TriangleMeshShaper::~TriangleMeshShaper() { delete impl; } /*! @if jp プリミティブ形状ã®ãƒ¡ãƒƒã‚·ãƒ¥åŒ–時ã«ãŠã‘る分割数ã®æŒ‡å®šã€‚ デフォルトã®åˆ†å‰²æ•°ã¯20ã¨ã—ã¦ã„る。 @endif */ void TriangleMeshShaper::setDivisionNumber(int n) { impl->divisionNumber = std::max(4, n); } /*! @if jp 整形時ã«æ³•ç·šã‚‚生æˆã™ã‚‹ã‹ã©ã†ã‹ã‚’指定ã™ã‚‹ @endif */ void TriangleMeshShaper::setNormalGenerationMode(bool on) { impl->isNormalGenerationMode = on; } /*! @if jp 変æ›å¾Œã®Shapeノードã«å¯¾ã—ã¦ã€å¤‰æ›å‰ã®ãƒŽãƒ¼ãƒ‰ãŒæŒã£ã¦ã„ãŸGeometryNodeã‚’è¿”ã™ã€‚ 例ãˆã°ã€å…ƒã®GeometryãŒãƒ—リミティブ形å¼ã ã£ãŸå ´åˆã€ãƒ—リミティブã®ç¨®é¡žã‚„パラメータを知るã“ã¨ãŒå‡ºæ¥ã‚‹ã€‚ @endif */ SFNode TriangleMeshShaper::getOriginalGeometry(VrmlShapePtr shapeNode) { return impl->getOriginalGeometry(shapeNode); } SFNode TMSImpl::getOriginalGeometry(VrmlShapePtr shapeNode) { SFNode originalGeometryNode; ShapeToGeometryMap::iterator p = shapeToOriginalGeometryMap.find(shapeNode); if(p != shapeToOriginalGeometryMap.end()){ originalGeometryNode = p->second; } return originalGeometryNode; } /*! @if jp æ•´å½¢å‡¦ç† (ModelNodeSet内ã®Shape) 引数ã§ä¸Žãˆã‚‰ã‚ŒãŸãƒŽãƒ¼ãƒ‰ã‚’トップã¨ã™ã‚‹ã‚·ãƒ¼ãƒ³ã‚°ãƒ©ãƒ•ä¸­ã® Shape ノードを IndexedFaceSetå½¢å¼ã«ãŠã„ã¦ã™ã¹ã¦ã®ãƒãƒªã‚´ãƒ³ã‚’三角形ã¨ã—ãŸçµ±ä¸€çš„ãªå¹¾ä½•å½¢çŠ¶å½¢å¼ã«å¤‰æ›ã™ã‚‹. 統一幾何形状形å¼ã«å¤‰æ›ã§ããªã„ノードã¯å‰Šé™¤ã•ã‚Œã‚‹ã€‚ @return 変æ›å¾Œã®ãƒˆãƒƒãƒ—ノード。 トップノードãŒShapeノードã®ã¨ãã«çµ±ä¸€å¹¾ä½•å½¢çŠ¶å½¢å¼ã¸ã¨å¤‰æ›å‡ºæ¥ãªã‹ã£ãŸå ´åˆã¯ç„¡åŠ¹ãªãƒã‚¤ãƒ³ã‚¿ã‚’è¿”ã™ã€‚ @endif */ VrmlNodePtr TriangleMeshShaper::apply(VrmlNodePtr topNode) { bool resultOfTopNode = impl->traverseShapeNodes(topNode.get(), 0, 0); return resultOfTopNode ? topNode : VrmlNodePtr(); } bool TMSImpl::traverseShapeNodes(VrmlNode* node, AbstractVrmlGroup* parentNode, int indexInParent) { bool result = true; if(node->isCategoryOf(PROTO_INSTANCE_NODE)){ VrmlProtoInstance* protoInstance = static_cast(node); if(protoInstance->actualNode){ traverseShapeNodes(protoInstance->actualNode.get(), parentNode, indexInParent); } } else if(node->isCategoryOf(GROUPING_NODE)){ AbstractVrmlGroup* group = static_cast(node); int numChildren = group->countChildren(); for(int i = 0; i < numChildren; i++){ traverseShapeNodes(group->getChild(i), group, i); } } else if(node->isCategoryOf(SHAPE_NODE)){ VrmlShape* shapeNode = static_cast(node); result = convertShapeNode(shapeNode); if(!result){ if(parentNode){ putMessage("Node is inconvertible and removed from the scene graph"); parentNode->removeChild(indexInParent); } } } return result; } bool TMSImpl::convertShapeNode(VrmlShape* shapeNode) { bool result = false; VrmlNode *node = shapeNode->geometry.get(); VrmlGeometry* geometry = dynamic_cast(node); if (!geometry){ VrmlProtoInstance *protoInstance = dynamic_cast(node); if (protoInstance){ geometry = dynamic_cast(protoInstance->actualNode.get()); } } VrmlIndexedFaceSetPtr triangleMesh; if(VrmlIndexedFaceSet* faceSet = dynamic_cast(geometry)){ if(faceSet->coord){ result = convertIndexedFaceSet(faceSet); triangleMesh = faceSet; } } else { triangleMesh = new VrmlIndexedFaceSet(); triangleMesh->coord = new VrmlCoordinate(); if(VrmlBox* box = dynamic_cast(geometry)){ result = convertBox(box, triangleMesh); } else if(VrmlCone* cone = dynamic_cast(geometry)){ result = convertCone(cone, triangleMesh); } else if(VrmlCylinder* cylinder = dynamic_cast(geometry)){ result = convertCylinder(cylinder, triangleMesh); } else if(VrmlSphere* sphere = dynamic_cast(geometry)){ result = convertSphere(sphere, triangleMesh); } else if(VrmlElevationGrid* elevationGrid = dynamic_cast(geometry)){ result = convertElevationGrid(elevationGrid, triangleMesh); } else if(VrmlExtrusion* extrusion = dynamic_cast(geometry)){ result = convertExtrusion(extrusion, triangleMesh); } if(result){ shapeToOriginalGeometryMap[shapeNode] = node; shapeNode->geometry = triangleMesh; } } if(result && !triangleMesh->normal && isNormalGenerationMode){ generateNormals(triangleMesh); } return result; } bool TMSImpl::convertIndexedFaceSet(VrmlIndexedFaceSet* faceSet) { MFVec3f& vertices = faceSet->coord->point; int numVertices = vertices.size(); MFInt32& indices = faceSet->coordIndex; const MFInt32 orgIndices = indices; indices.clear(); const int numOrgIndices = orgIndices.size(); int orgFaceIndex = 0; int polygonTopIndexPosition = 0; indexPositionMap.clear(); faceIndexMap.clear(); polygon.clear(); int triangleOrder[3]; if(faceSet->ccw){ triangleOrder[0] = 0; triangleOrder[1] = 1; triangleOrder[2] = 2; } else { triangleOrder[0] = 2; triangleOrder[1] = 1; triangleOrder[2] = 0; } triangulator.setVertices(vertices); for(int i=0; i < numOrgIndices; ++i){ int index = orgIndices[i]; if(index >= numVertices){ putMessage("The coordIndex field has an index over the size of the vertices in the coord field"); } else if(index >= 0){ polygon.push_back(index); } else { int numTriangles = triangulator.apply(polygon); const vector& triangles = triangulator.triangles(); for(int j=0; j < numTriangles; ++j){ for(int k=0; k < 3; ++k){ int localIndex = triangles[j * 3 + triangleOrder[k]]; indices.push_back(polygon[localIndex]); indexPositionMap.push_back(polygonTopIndexPosition + localIndex); } indices.push_back(-1); indexPositionMap.push_back(-1); faceIndexMap.push_back(orgFaceIndex); } polygonTopIndexPosition = i + 1; orgFaceIndex++; polygon.clear(); } } bool result = true; int numColors = faceSet->color ? faceSet->color->color.size() : 0; result &= checkAndRemapIndices (REMAP_COLOR, numColors, faceSet->colorIndex, faceSet->colorPerVertex, faceSet); int numNormals = faceSet->normal ? faceSet->normal->vector.size() : 0; result &= checkAndRemapIndices (REMAP_NORMAL, numNormals, faceSet->normalIndex, faceSet->normalPerVertex, faceSet); if(numNormals > 0 && !faceSet->ccw){ // flip normal vectors MFVec3f& normals = faceSet->normal->vector; for(size_t i=0; i < normals.size(); ++i){ normals[i] = -normals[i]; } } faceSet->ccw = true; setTexCoordIndex(faceSet); return (result && !indices.empty()); } template bool TMSImpl::remapDirectMapObjectsPerFaces(TArray& values, const char* valueName) { const TArray orgValues = values; int numOrgValues = orgValues.size(); int numFaces = faceIndexMap.size(); values.resize(numFaces); for(int i=0; i < numFaces; ++i){ int faceIndex = faceIndexMap[i]; if(faceIndex >= numOrgValues){ putMessage(string("The number of ") + valueName + " is less than the number of faces."); return false; } values[i] = orgValues[faceIndex]; } return true; } bool TMSImpl::checkAndRemapIndices (RemapType type, int numElements, MFInt32& indices, bool perVertex, VrmlIndexedFaceSet* triangleMesh) { const char* valueName = (type==REMAP_COLOR) ? "colors" : "normals" ; bool result = true; if(numElements == 0){ if(!indices.empty()){ putMessage(string("An IndexedFaceSet has no ") + valueName + ", but it has a non-empty index field of " + valueName + "."); result = false; } } else { if(indices.empty()){ if(perVertex){ if(numElements < static_cast(triangleMesh->coord->point.size())){ putMessage(string("The number of ") + valueName + " is less than the number of vertices."); result = false; } } else { if(type == REMAP_COLOR){ remapDirectMapObjectsPerFaces(triangleMesh->color->color, valueName); } else if(type == REMAP_NORMAL){ remapDirectMapObjectsPerFaces(triangleMesh->normal->vector, valueName); } } } else { const MFInt32 orgIndices = indices; if(perVertex){ int numNewIndices = indexPositionMap.size(); indices.resize(numNewIndices); for(int i=0; i < numNewIndices; ++i){ int orgPosition = indexPositionMap[i]; if(orgPosition < 0){ indices[i] = -1; } else { int index = orgIndices[orgPosition]; if(index < numElements){ indices[i] = index; } else { putError1(valueName); result = false; } } } } else { int numNewIndices = faceIndexMap.size(); indices.resize(numNewIndices); for(int i=0; i < numNewIndices; ++i){ int orgFaceIndex = faceIndexMap[i]; int index = orgIndices[orgFaceIndex]; if(index < numElements){ indices[i] = index; } else { putError1(valueName); result = false; } } } } } return result; } bool TMSImpl::setTexCoordIndex(VrmlIndexedFaceSetPtr faseSet) { bool result = true; VrmlTextureCoordinatePtr texCoord = faseSet->texCoord; MFInt32& texCoordIndex = faseSet->texCoordIndex; MFInt32& coordIndex = faseSet->coordIndex; if(texCoord){ if(texCoordIndex.empty()){ texCoordIndex.resize(coordIndex.size()); copy( coordIndex.begin(), coordIndex.end(), texCoordIndex.begin() ); } else { const MFInt32 orgIndices = texCoordIndex; int numNewIndices = indexPositionMap.size(); texCoordIndex.resize(numNewIndices); for(int i=0; i < numNewIndices; ++i){ if(indexPositionMap[i] == -1){ texCoordIndex[i] = -1; } else { int index = orgIndices[indexPositionMap[i]]; if(index < (int)texCoord->point.size()){ texCoordIndex[i] = index; } else { putError1("texCoordIndex"); result = false; } } } } } return result; } void TMSImpl::putError1(const char* valueName) { putMessage(string("There is an index of ") + valueName + " beyond the size of " + valueName + "."); } namespace { inline void addTriangle(MFInt32& indices, int x, int y, int z) { indices.push_back(x); indices.push_back(y); indices.push_back(z); indices.push_back(-1); } } bool TMSImpl::convertBox(VrmlBox* box, VrmlIndexedFaceSetPtr& triangleMesh) { const double x = box->size[0] / 2.0; const double y = box->size[1] / 2.0; const double z = box->size[2] / 2.0; if(x < 0.0 || y < 0.0 || z < 0.0 ){ putMessage("BOX : wrong value."); return false; } MFVec3f& vertices = triangleMesh->coord->point; vertices.reserve(8); static const int numTriangles = 12; static const double xsigns[] = { -1.0, -1.0, -1.0, -1.0, 1.0, 1.0, 1.0, 1.0 }; static const double ysigns[] = { -1.0, -1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 1.0 }; static const double zsigns[] = { -1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 1.0, -1.0 }; static const int triangles[] = { 0, 1, 2, 2, 3, 0, 3, 2, 6, 3, 6, 7, 6, 2, 1, 1, 5, 6, 1, 0, 5, 5, 0, 4, 5, 4, 6, 6, 4, 7, 7, 4, 0, 0, 3, 7 }; for(int i=0; i < 8; ++i){ vertices.push_back(Vector3(xsigns[i] * x, ysigns[i] * y, zsigns[i] * z)); } MFInt32& indices = triangleMesh->coordIndex; indices.resize(numTriangles * 4); int di = 0; int si = 0; for(int i=0; i < numTriangles; i++){ indices[di++] = triangles[si++]; indices[di++] = triangles[si++]; indices[di++] = triangles[si++]; indices[di++] = -1; } return true; } bool TMSImpl::convertCone(VrmlCone* cone, VrmlIndexedFaceSetPtr& triangleMesh) { const double radius = cone->bottomRadius; if(cone->height < 0.0 || radius < 0.0 ){ putMessage( "CONE : wrong value." ); return false; } MFVec3f& vertices = triangleMesh->coord->point; vertices.reserve(divisionNumber + 1); for(int i=0; i < divisionNumber; ++i){ const double angle = i * 2.0 * PI / divisionNumber; vertices.push_back(Vector3(radius * cos(angle), -cone->height/2.0, radius * sin(angle))); } const int topIndex = vertices.size(); vertices.push_back(Vector3(0.0, cone->height / 2.0, 0.0)); const int bottomCenterIndex = vertices.size(); vertices.push_back(Vector3(0.0, -cone->height / 2.0, 0.0)); MFInt32& indices = triangleMesh->coordIndex; indices.reserve((divisionNumber * 2) * 4); for(int i=0; i < divisionNumber; ++i){ // side faces if(cone->side){ addTriangle(indices, topIndex, (i + 1) % divisionNumber, i); } // bottom faces if(cone->bottom){ addTriangle(indices, bottomCenterIndex, i, (i + 1) % divisionNumber); } } triangleMesh->creaseAngle = 3.14 / 2.0; return true; } bool TMSImpl::convertCylinder(VrmlCylinder* cylinder, VrmlIndexedFaceSetPtr& triangleMesh) { if(cylinder->height < 0.0 || cylinder->radius < 0.0){ putMessage("CYLINDER : wrong value."); return false; } MFVec3f& vertices = triangleMesh->coord->point; vertices.reserve(divisionNumber * 2 + 2); vertices.resize(divisionNumber * 2); const double y = cylinder->height / 2.0; for(int i=0 ; i < divisionNumber ; i++ ){ const double angle = i * 2.0 * PI / divisionNumber; SFVec3f& vtop = vertices[i]; SFVec3f& vbottom = vertices[i + divisionNumber]; vtop[0] = vbottom[0] = cylinder->radius * cos(angle); vtop[2] = vbottom[2] = cylinder->radius * sin(angle); vtop[1] = y; vbottom[1] = -y; } const int topCenterIndex = vertices.size(); vertices.push_back(Vector3(0.0, y, 0.0)); const int bottomCenterIndex = vertices.size(); vertices.push_back(Vector3(0.0, -y, 0.0)); MFInt32& indices = triangleMesh->coordIndex; indices.reserve((divisionNumber * 4) * 4); for(int i=0; i < divisionNumber; ++i){ // top face if(cylinder->top){ addTriangle(indices, topCenterIndex, (i+1) % divisionNumber, i); } // side face (upward convex triangle) if(cylinder->side){ addTriangle(indices, i, ((i+1) % divisionNumber) + divisionNumber, i + divisionNumber); // side face (downward convex triangle) addTriangle(indices, i, (i+1) % divisionNumber, ((i + 1) % divisionNumber) + divisionNumber); } // bottom face if(cylinder->bottom){ addTriangle(indices, bottomCenterIndex, i + divisionNumber, ((i+1) % divisionNumber) + divisionNumber); } } triangleMesh->creaseAngle = 3.14 / 2.0; return true; } bool TMSImpl::convertSphere(VrmlSphere* sphere, VrmlIndexedFaceSetPtr& triangleMesh) { const double r = sphere->radius; if(r < 0.0) { putMessage("SPHERE : wrong value."); return false; } const int vdn = divisionNumber / 2; // latitudinal division number const int hdn = divisionNumber; // longitudinal division number MFVec3f& vertices = triangleMesh->coord->point; vertices.reserve((vdn - 1) * hdn + 2); for(int i=1; i < vdn; i++){ // latitudinal direction double tv = i * PI / vdn; for(int j=0; j < hdn; j++){ // longitudinal direction double th = j * 2.0 * PI / hdn; vertices.push_back(Vector3(r * sin(tv) * cos(th), r * cos(tv), r * sin(tv) * sin(th))); } } const int topIndex = vertices.size(); vertices.push_back(Vector3(0.0, r, 0.0)); const int bottomIndex = vertices.size(); vertices.push_back(Vector3(0.0, -r, 0.0)); MFInt32& indices = triangleMesh->coordIndex; indices.reserve(vdn * hdn * 2 * 4); // top faces for(int i=0; i < hdn; ++i){ addTriangle(indices, topIndex, (i+1) % hdn, i); } // side faces for(int i=0; i < vdn - 2; ++i){ const int upper = i * hdn; const int lower = (i + 1) * hdn; for(int j=0; j < hdn; ++j) { // upward convex triangle addTriangle(indices, j + upper, ((j + 1) % hdn) + lower, j + lower); // downward convex triangle addTriangle(indices, j + upper, ((j + 1) % hdn) + upper, ((j + 1) % hdn) + lower); } } // bottom faces const int offset = (vdn - 2) * hdn; for(int i=0; i < hdn; ++i){ addTriangle(indices, bottomIndex, (i % hdn) + offset, ((i+1) % hdn) + offset); } triangleMesh->creaseAngle = PI; return true; } /** \todo copy colors and color indices to triangleMesh */ bool TMSImpl::convertElevationGrid(VrmlElevationGrid* grid, VrmlIndexedFaceSetPtr& triangleMesh) { if(grid->xDimension * grid->zDimension != static_cast(grid->height.size())){ putMessage("ELEVATIONGRID : wrong value."); return false; } MFVec3f& vertices = triangleMesh->coord->point; vertices.reserve(grid->zDimension * grid->xDimension); for(int z=0; z < grid->zDimension; z++){ for(int x=0; x < grid->xDimension; x++ ){ vertices.push_back(Vector3(x * grid->xSpacing, grid->height[z * grid->xDimension + x], z * grid->zSpacing)); } } MFInt32& indices = triangleMesh->coordIndex; indices.reserve((grid->zDimension - 1) * (grid->xDimension - 1) * 2 * 4); for(int z=0; z < grid->zDimension - 1; ++z){ const int current = z * grid->xDimension; const int next = (z + 1) * grid->xDimension; for(int x=0; x < grid->xDimension - 1; ++x){ if(grid->ccw){ addTriangle(indices, x + current, x + next, (x + 1) + next); addTriangle(indices, x + current, (x + 1) + next, (x + 1) + current); }else{ addTriangle(indices, x + current, (x + 1) + next, x + next); addTriangle(indices, x + current, (x + 1) + current, (x + 1) + next); } } } triangleMesh->creaseAngle = grid->creaseAngle; triangleMesh->ccw = true; //triangleMesh->normalPerVertex = grid->normalPerVertex; triangleMesh->solid = grid->solid; if(grid->texCoord){ triangleMesh->texCoord->point.resize(grid->texCoord->point.size()); copy(grid->texCoord->point.begin(), grid->texCoord->point.end(), triangleMesh->texCoord->point.begin()); triangleMesh->texCoordIndex.resize(indices.size()); copy(indices.begin(), indices.end(), triangleMesh->texCoordIndex.begin()); } return true; } bool TMSImpl::convertExtrusion(VrmlExtrusion* extrusion, VrmlIndexedFaceSetPtr& triangleMesh) { bool isClosed = false; int numSpine = extrusion->spine.size(); int numcross = extrusion->crossSection.size(); if(extrusion->spine[0][0] == extrusion->spine[numSpine - 1][0] && extrusion->spine[0][1] == extrusion->spine[numSpine - 1][1] && extrusion->spine[0][2] == extrusion->spine[numSpine - 1][2] ){ isClosed = true; } bool crossSectionisClosed = false; if(extrusion->crossSection[0][0] == extrusion->crossSection[numcross - 1][0] && extrusion->crossSection[0][1] == extrusion->crossSection[numcross - 1][1] ){ crossSectionisClosed = true; } MFVec3f& vertices = triangleMesh->coord->point; vertices.reserve(numSpine*numcross); SFVec3f preZaxis(SFVec3f::Zero()); int definedZaxis = -1; std::vector Yaxisarray; std::vector Zaxisarray; if(numSpine > 2){ for(int i=0; i < numSpine; ++i){ SFVec3f Yaxis, Zaxis; if(i == 0){ if(isClosed){ const SFVec3f& spine1 = extrusion->spine[numSpine - 2]; const SFVec3f& spine2 = extrusion->spine[0]; const SFVec3f& spine3 = extrusion->spine[1]; Yaxis = spine3 - spine1; Zaxis = (spine3 - spine2).cross(spine1 - spine2); } else { const SFVec3f& spine1 = extrusion->spine[0]; const SFVec3f& spine2 = extrusion->spine[1]; const SFVec3f& spine3 = extrusion->spine[2]; Yaxis = spine2 - spine1; Zaxis = (spine3 - spine2).cross(spine1 - spine2); } } else if(i == numSpine - 1){ if(isClosed){ const SFVec3f& spine1 = extrusion->spine[numSpine - 2]; const SFVec3f& spine2 = extrusion->spine[0]; const SFVec3f& spine3 = extrusion->spine[1]; Yaxis = spine3 - spine1; Zaxis = (spine3 - spine2).cross(spine1 - spine2); } else { const SFVec3f& spine1 = extrusion->spine[numSpine - 3]; const SFVec3f& spine2 = extrusion->spine[numSpine - 2]; const SFVec3f& spine3 = extrusion->spine[numSpine - 1]; Yaxis = spine3 - spine2; Zaxis = (spine3 - spine2).cross(spine1 - spine2); } } else { const SFVec3f& spine1 = extrusion->spine[i - 1]; const SFVec3f& spine2 = extrusion->spine[i]; const SFVec3f& spine3 = extrusion->spine[i + 1]; Yaxis = spine3 - spine1; Zaxis = (spine3-spine2).cross(spine1-spine2); } if(!Zaxis.norm()){ if(definedZaxis != -1) Zaxis = preZaxis; } else { if(definedZaxis == -1){ definedZaxis = i; } preZaxis = Zaxis; } Yaxisarray.push_back(Yaxis); Zaxisarray.push_back(Zaxis); } } else { const SFVec3f Yaxis(extrusion->spine[1] - extrusion->spine[0]); Yaxisarray.push_back(Yaxis); Yaxisarray.push_back(Yaxis); } for(int i=0; i < numSpine; ++i){ Eigen::Matrix3d Scp; SFVec3f y = Yaxisarray[i].normalized(); if(definedZaxis == -1){ SFRotation R(acos(y[1]), SFRotation::Vector3(y[2], 0.0, -y[0])); Scp = R.toRotationMatrix(); } else { if(i < definedZaxis){ Zaxisarray[i] = Zaxisarray[definedZaxis]; } if(i && (Zaxisarray[i].dot(Zaxisarray[i - 1]) < 0.0)){ Zaxisarray[i] *= -1.0; } SFVec3f z = Zaxisarray[i].normalized(); SFVec3f x = y.cross(z); Scp << x, y, z; } const SFVec3f& spine = extrusion->spine[i]; SFVec3f scale; if(extrusion->scale.size() == 1){ scale << extrusion->scale[0][0], 0.0, extrusion->scale[0][1]; } else { scale << extrusion->scale[i][0], 0.0, extrusion->scale[i][1]; } SFRotation o; if(extrusion->orientation.size() == 1){ o = extrusion->orientation[0]; } else { o = extrusion->orientation[i]; } for(int j=0; j < numcross; ++j){ const SFVec3f crossSection(extrusion->crossSection[j][0], 0.0, extrusion->crossSection[j][1]); const SFVec3f v1(crossSection[0] * scale[0], 0.0, crossSection[2] * scale[2]); vertices.push_back(Scp * o.toRotationMatrix() * v1 + spine); } } MFInt32& indices = triangleMesh->coordIndex; for(int i=0; i < numSpine - 1 ; ++i){ const int upper = i * numcross; const int lower = (i + 1) * numcross; for(int j=0; j < numcross - 1; ++j) { if(extrusion->ccw){ // upward convex triangle addTriangle(indices, j + upper, (j + 1) + lower, j + lower); // downward convex triangle addTriangle(indices, j + upper, (j + 1) + upper, j + 1 + lower); } else { addTriangle(indices, j + upper, j + lower, (j + 1) + lower); addTriangle(indices, j + upper, (j + 1) + lower, j + 1 + upper); } } } int j = 0; if(crossSectionisClosed){ j=1; } if(extrusion->beginCap && !isClosed){ triangulator.setVertices(vertices); polygon.clear(); for(int i=0; i < numcross - j; ++i){ polygon.push_back(i); } triangulator.apply(polygon); const vector& triangles = triangulator.triangles(); for(size_t i=0; i < triangles.size(); i += 3){ if(extrusion->ccw){ addTriangle(indices, polygon[triangles[i]], polygon[triangles[i+1]], polygon[triangles[i+2]]); } else { addTriangle(indices, polygon[triangles[i]], polygon[triangles[i+2]], polygon[triangles[i+1]]); } } } if(extrusion->endCap && !isClosed){ triangulator.setVertices(vertices); polygon.clear(); for(int i=0; i < numcross - j; ++i){ polygon.push_back(numcross * (numSpine - 1) + i); } triangulator.apply(polygon); const vector& triangles = triangulator.triangles(); for(size_t i=0; i < triangles.size(); i +=3){ if(extrusion->ccw){ addTriangle(indices, polygon[triangles[i]], polygon[triangles[i+1]], polygon[triangles[i+2]]); } else { addTriangle(indices, polygon[triangles[i]], polygon[triangles[i+2]], polygon[triangles[i+1]]); } } } triangleMesh->creaseAngle = extrusion->creaseAngle; triangleMesh->ccw = true; triangleMesh->convex = true; triangleMesh->solid = extrusion->solid; return true; } void TMSImpl::generateNormals(VrmlIndexedFaceSetPtr& triangleMesh) { triangleMesh->normal = new VrmlNormal(); triangleMesh->normalPerVertex = (triangleMesh->creaseAngle > 0.0) ? true : false; calculateFaceNormals(triangleMesh); if(triangleMesh->normalPerVertex){ setVertexNormals(triangleMesh); } else { setFaceNormals(triangleMesh); } } void TMSImpl::calculateFaceNormals(VrmlIndexedFaceSetPtr& triangleMesh) { const MFVec3f& vertices = triangleMesh->coord->point; const int numVertices = vertices.size(); const MFInt32& triangles = triangleMesh->coordIndex; const int numFaces = triangles.size() / 4; faceNormals.clear(); if(triangleMesh->normalPerVertex){ vertexIndexToFaceIndicesMap.clear(); vertexIndexToFaceIndicesMap.resize(numVertices); } for(int faceIndex=0; faceIndex < numFaces; ++faceIndex){ const SFVec3f& v0 = vertices[triangles[faceIndex * 4 + 0]]; const SFVec3f& v1 = vertices[triangles[faceIndex * 4 + 1]]; const SFVec3f& v2 = vertices[triangles[faceIndex * 4 + 2]]; const SFVec3f normal((v1 - v0).cross(v2 - v0).normalized()); faceNormals.push_back(normal); if(triangleMesh->normalPerVertex){ for(int i=0; i < 3; ++i){ int vertexIndex = triangles[faceIndex * 4 + i]; vector& facesOfVertex = vertexIndexToFaceIndicesMap[vertexIndex]; bool isSameNormalFaceFound = false; for(size_t j=0; j < facesOfVertex.size(); ++j){ const SFVec3f& otherNormal = faceNormals[facesOfVertex[j]]; // the same face is not appended if((otherNormal - normal).squaredNorm() <= numeric_limits::epsilon()){ isSameNormalFaceFound = true; break; } } if(!isSameNormalFaceFound){ facesOfVertex.push_back(faceIndex); } } } } } void TMSImpl::setVertexNormals(VrmlIndexedFaceSetPtr& triangleMesh) { const MFVec3f& vertices = triangleMesh->coord->point; const int numVertices = vertices.size(); const MFInt32& triangles = triangleMesh->coordIndex; const int numFaces = triangles.size() / 4; MFVec3f& normals = triangleMesh->normal->vector; MFInt32& normalIndices = triangleMesh->normalIndex; normalIndices.clear(); normalIndices.reserve(triangles.size()); vertexIndexToNormalIndicesMap.clear(); vertexIndexToNormalIndicesMap.resize(numVertices); // const double cosCreaseAngle = cos(triangleMesh->creaseAngle); for(int faceIndex=0; faceIndex < numFaces; ++faceIndex){ for(int i=0; i < 3; ++i){ int vertexIndex = triangles[faceIndex * 4 + i]; vector& facesOfVertex = vertexIndexToFaceIndicesMap[vertexIndex]; const SFVec3f& currentFaceNormal = faceNormals[faceIndex]; SFVec3f normal = currentFaceNormal; bool normalIsFaceNormal = true; // avarage normals of the faces whose crease angle is below the 'creaseAngle' variable for(size_t j=0; j < facesOfVertex.size(); ++j){ int adjoingFaceIndex = facesOfVertex[j]; const SFVec3f& adjoingFaceNormal = faceNormals[adjoingFaceIndex]; double angle = acos(currentFaceNormal.dot(adjoingFaceNormal) / (currentFaceNormal.norm() * adjoingFaceNormal.norm())); if(angle > 1.0e-6 && angle < triangleMesh->creaseAngle){ normal += adjoingFaceNormal; normalIsFaceNormal = false; } } if(!normalIsFaceNormal){ normal.normalize(); } int normalIndex = -1; for(int j=0; j < 3; ++j){ int vertexIndex2 = triangles[faceIndex * 4 + j]; const vector& normalIndicesOfVertex = vertexIndexToNormalIndicesMap[vertexIndex2]; for(size_t k=0; k < normalIndicesOfVertex.size(); ++k){ int index = normalIndicesOfVertex[k]; if((normals[index] - normal).squaredNorm() <= numeric_limits::epsilon()){ normalIndex = index; goto normalIndexFound; } } } if(normalIndex < 0){ normalIndex = normals.size(); normals.push_back(normal); vertexIndexToNormalIndicesMap[vertexIndex].push_back(normalIndex); } normalIndexFound: normalIndices.push_back(normalIndex); } normalIndices.push_back(-1); } } void TMSImpl::setFaceNormals(VrmlIndexedFaceSetPtr& triangleMesh) { const MFInt32& triangles = triangleMesh->coordIndex; const int numFaces = triangles.size() / 4; MFVec3f& normals = triangleMesh->normal->vector; MFInt32& normalIndices = triangleMesh->normalIndex; normalIndices.clear(); normalIndices.reserve(numFaces); const int numVertices = triangleMesh->coord->point.size(); vertexIndexToNormalIndicesMap.clear(); vertexIndexToNormalIndicesMap.resize(numVertices); for(int faceIndex=0; faceIndex < numFaces; ++faceIndex){ const SFVec3f& normal = faceNormals[faceIndex]; int normalIndex = -1; // find the same normal from the existing normals for(int i=0; i < 3; ++i){ int vertexIndex = triangles[faceIndex * 4 + i]; vector& normalIndicesOfVertex = vertexIndexToNormalIndicesMap[vertexIndex]; for(size_t j=0; j < normalIndicesOfVertex.size(); ++j){ int index = normalIndicesOfVertex[j]; if((normals[index] - normal).norm() <= numeric_limits::epsilon()){ normalIndex = index; goto normalIndexFound2; } } } if(normalIndex < 0){ normalIndex = normals.size(); normals.push_back(normal); for(int i=0; i < 3; ++i){ int vertexIndex = triangles[faceIndex * 4 + i]; vertexIndexToNormalIndicesMap[vertexIndex].push_back(normalIndex); } } normalIndexFound2: normalIndices.push_back(normalIndex); } } void TMSImpl::putMessage(const std::string& message) { if(!self->sigMessage.empty()){ self->sigMessage(message + "\n" ); } } void TriangleMeshShaper::defaultTextureMapping(VrmlShape* shapeNode) { VrmlIndexedFaceSet* triangleMesh = dynamic_cast(shapeNode->geometry.get()); if(!triangleMesh){ return; } VrmlNode* node = getOriginalGeometry(shapeNode).get(); VrmlGeometry* originalGeometry = dynamic_cast(node); if (!originalGeometry){ VrmlProtoInstance* protoInstance = dynamic_cast(node); if(protoInstance){ originalGeometry = dynamic_cast(protoInstance->actualNode.get()); } } if(originalGeometry){ if(dynamic_cast(originalGeometry)){ //Box defaultTextureMappingBox(triangleMesh); } else if(dynamic_cast(originalGeometry)){ //cone defaultTextureMappingCone(triangleMesh); } else if(dynamic_cast(originalGeometry)){ //Cylinder defaultTextureMappingCylinder(triangleMesh); } else if(VrmlSphere* sphere = dynamic_cast(originalGeometry)){ //sphere defaultTextureMappingSphere(triangleMesh, sphere->radius); } else if(VrmlElevationGrid* grid = dynamic_cast(originalGeometry)){ //ElevationGrid defaultTextureMappingElevationGrid(grid, triangleMesh); } else if(VrmlExtrusion* extrusion = dynamic_cast(originalGeometry)){ //Extrusion defaultTextureMappingExtrusion(triangleMesh, extrusion); } } else { //IndexedFaceSet defaultTextureMappingFaceSet(triangleMesh); } } void TriangleMeshShaper::defaultTextureMappingFaceSet(VrmlIndexedFaceSet* triangleMesh) { if(!triangleMesh->texCoord){ double max[3]; double min[3]; for(int i=0; i < 3; ++i){ max[i] = triangleMesh->coord->point[0][i]; min[i] = triangleMesh->coord->point[0][i]; } int n = triangleMesh->coord->point.size(); for(int i=1; icoord->point[i][j]; max[j] = std::max(max[j], w); min[j] = std::min(min[j], w); } } double size[3] = {0.0, 0.0, 0.0}; for(int j=0; j < 3; j++){ size[j] = max[j] - min[j]; } int s,t; size[0] >= size[1] ? ( size[0] >= size[2] ? ( s=0 , t=size[1] >= size[2] ? 1 : 2 ) : ( s=2 , t=0) ) : ( size[1] >= size[2] ? ( s=1 , t=size[0] >= size[2] ? 0 : 2 ) : ( s=2 , t=1) ) ; triangleMesh->texCoord = new VrmlTextureCoordinate(); double ratio = size[t] / size[s]; for(int i=0; icoord->point[i][s]-min[s]) / size[s]; point[1] = (triangleMesh->coord->point[i][t]-min[t]) / size[t] * ratio; triangleMesh->texCoord->point.push_back(point); } triangleMesh->texCoordIndex.resize(triangleMesh->coordIndex.size()); copy(triangleMesh->coordIndex.begin(), triangleMesh->coordIndex.end(), triangleMesh->texCoordIndex.begin()); } } void TriangleMeshShaper::defaultTextureMappingElevationGrid(VrmlElevationGrid* grid, VrmlIndexedFaceSet* triangleMesh) { double xmax = grid->xSpacing * (grid->xDimension-1); double zmax = grid->zSpacing * (grid->zDimension-1); triangleMesh->texCoord = new VrmlTextureCoordinate(); for(size_t i=0; icoord->point.size(); i++){ SFVec2f point; point[0] = (triangleMesh->coord->point[i][0]) / xmax; point[1] = (triangleMesh->coord->point[i][2]) / zmax; triangleMesh->texCoord->point.push_back(point); } triangleMesh->texCoordIndex.resize(triangleMesh->coordIndex.size()); copy(triangleMesh->coordIndex.begin(), triangleMesh->coordIndex.end(), triangleMesh->texCoordIndex.begin()); } int TriangleMeshShaper::faceofBox(SFVec3f* point) { if(point[0][0] <= 0 && point[1][0] <= 0 && point[2][0] <= 0 ) return LEFT; if(point[0][0] > 0 && point[1][0] > 0 && point[2][0] > 0 ) return RIGHT; if(point[0][1] <= 0 && point[1][1] <= 0 && point[2][1] <= 0 ) return BOTTOM; if(point[0][1] > 0 && point[1][1] > 0 && point[2][1] > 0 ) return TOP; if(point[0][2] <= 0 && point[1][2] <= 0 && point[2][2] <= 0 ) return BACK; if(point[0][2] > 0 && point[1][2] > 0 && point[2][2] > 0 ) return FRONT; return -1; } int TriangleMeshShaper::findPoint(MFVec2f& points, SFVec2f& target) { for(size_t i=0; i < points.size(); ++i){ if((points[i][0] - target[0]) * (points[i][0] - target[0]) + (points[i][1] - target[1]) * (points[i][1] - target[1]) < 1.0e-9){ return i; } } return -1; } double TriangleMeshShaper::calcangle(SFVec3f& point) { double angle = atan2(point[2], point[0]); if(angle>=0){ angle = 1.5 * PI - angle; } else if(-0.5 * PI < angle){ angle = -angle + 1.5 * PI; } else { angle = -angle - 0.5 *PI; } return angle; } void TriangleMeshShaper::defaultTextureMappingBox(VrmlIndexedFaceSet* triangleMesh) { triangleMesh->texCoord = new VrmlTextureCoordinate(); triangleMesh->texCoord->point.push_back(SFVec2f(0.0, 0.0)); //index=0 triangleMesh->texCoord->point.push_back(SFVec2f(1.0, 0.0)); //index=1 triangleMesh->texCoord->point.push_back(SFVec2f(0.0, 1.0)); //index=2 triangleMesh->texCoord->point.push_back(SFVec2f(1.0, 1.0)); //index=3 for(int i=0; i<12; i++){ SFVec3f point[3]; for(int j=0; j < 3; ++j){ point[j] = triangleMesh->coord->point[triangleMesh->coordIndex[i*4+j]]; } switch(faceofBox(point)){ case LEFT: for(int j=0; j < 3; ++j){ if(point[j][1] > 0 && point[j][2] > 0) triangleMesh->texCoordIndex.push_back(3); else if(point[j][1] > 0 && point[j][2] <= 0) triangleMesh->texCoordIndex.push_back(2); else if(point[j][1] <= 0 && point[j][2] > 0) triangleMesh->texCoordIndex.push_back(1); else if(point[j][1] <= 0 && point[j][2] <= 0) triangleMesh->texCoordIndex.push_back(0); } break; case RIGHT: for(int j=0; j < 3; ++j){ if(point[j][1] > 0 && point[j][2] > 0) triangleMesh->texCoordIndex.push_back(2); else if(point[j][1] > 0 && point[j][2] <= 0) triangleMesh->texCoordIndex.push_back(3); else if(point[j][1] <= 0 && point[j][2] > 0) triangleMesh->texCoordIndex.push_back(0); else if(point[j][1] <= 0 && point[j][2] <= 0) triangleMesh->texCoordIndex.push_back(1); } break; case BOTTOM: for(int j=0; j < 3; ++j){ if(point[j][2] > 0 && point[j][0] > 0) triangleMesh->texCoordIndex.push_back(3); else if(point[j][2] > 0 && point[j][0] <= 0) triangleMesh->texCoordIndex.push_back(2); else if(point[j][2] <= 0 && point[j][0] > 0) triangleMesh->texCoordIndex.push_back(1); else if(point[j][2] <= 0 && point[j][0] <= 0) triangleMesh->texCoordIndex.push_back(0); } break; case TOP: for(int j=0; j < 3; ++j){ if(point[j][2] > 0 && point[j][0] > 0) triangleMesh->texCoordIndex.push_back(1); else if(point[j][2] > 0 && point[j][0] <= 0) triangleMesh->texCoordIndex.push_back(0); else if(point[j][2] <= 0 && point[j][0] > 0) triangleMesh->texCoordIndex.push_back(3); else if(point[j][2] <= 0 && point[j][0] <= 0) triangleMesh->texCoordIndex.push_back(2); } break; case BACK: for(int j=0; j < 3; ++j){ if(point[j][1] > 0 && point[j][0] > 0) triangleMesh->texCoordIndex.push_back(2); else if(point[j][1] > 0 && point[j][0] <= 0) triangleMesh->texCoordIndex.push_back(3); else if(point[j][1] <= 0 && point[j][0] > 0) triangleMesh->texCoordIndex.push_back(0); else if(point[j][1] <= 0 && point[j][0] <= 0) triangleMesh->texCoordIndex.push_back(1); } break; case FRONT: for(int j=0; j < 3; ++j){ if(point[j][1] > 0 && point[j][0] > 0) triangleMesh->texCoordIndex.push_back(3); else if(point[j][1] > 0 && point[j][0] <= 0) triangleMesh->texCoordIndex.push_back(2); else if(point[j][1] <= 0 && point[j][0] > 0) triangleMesh->texCoordIndex.push_back(1); else if(point[j][1] <= 0 && point[j][0] <= 0) triangleMesh->texCoordIndex.push_back(0); } break; default: break; } triangleMesh->texCoordIndex.push_back(-1); } } void TriangleMeshShaper::defaultTextureMappingCone(VrmlIndexedFaceSet* triangleMesh) { triangleMesh->texCoord = new VrmlTextureCoordinate(); SFVec2f texPoint(0.5, 0.5); //center of bottom index=0 triangleMesh->texCoord->point.push_back(texPoint); int texIndex = 1; for(size_t i=0; i < triangleMesh->coordIndex.size(); ++i){ SFVec3f point[3]; int top = -1; int center = -1; for(int j=0; j < 3; ++j){ point[j] = triangleMesh->coord->point[triangleMesh->coordIndex[i++]]; if(point[j][1] > 0.0) top = j; if(point[j][0] == 0.0 && point[j][2] == 0.0) center = j; } if(top >= 0){ //side double s[3] = { 0.0, 0.0, 0.0}; int pre=-1; for(int j=0; j < 3; ++j){ if(j != top){ double angle = calcangle(point[j]); s[j] = angle / 2.0 / PI; if(pre!=-1) if(s[pre] > 0.5 && s[j] < 1.0e-6) s[j] = 1.0; pre = j; } } for(int j=0; j < 3; ++j){ if(j != top){ texPoint << s[j], 0.0; }else{ texPoint << (s[0] + s[1] + s[2]) / 2.0, 1.0; } int k=findPoint(triangleMesh->texCoord->point, texPoint); if(k != -1){ triangleMesh->texCoordIndex.push_back(k); }else{ triangleMesh->texCoord->point.push_back(texPoint); triangleMesh->texCoordIndex.push_back(texIndex++); } } triangleMesh->texCoordIndex.push_back(-1); }else{ // bottom for(int j=0; j < 3; ++j){ if(j != center){ double angle = atan2(point[j][2], point[j][0]); texPoint << 0.5 + 0.5 * cos(angle), 0.5 + 0.5 * sin(angle); int k=findPoint(triangleMesh->texCoord->point, texPoint); if(k != -1){ triangleMesh->texCoordIndex.push_back(k); }else{ triangleMesh->texCoord->point.push_back(texPoint); triangleMesh->texCoordIndex.push_back(texIndex++); } }else{ triangleMesh->texCoordIndex.push_back(0); } } triangleMesh->texCoordIndex.push_back(-1); } } } void TriangleMeshShaper::defaultTextureMappingCylinder(VrmlIndexedFaceSet* triangleMesh) { triangleMesh->texCoord = new VrmlTextureCoordinate(); SFVec2f texPoint(0.5, 0.5); // center of top(bottom) index=0 triangleMesh->texCoord->point.push_back(texPoint); int texIndex = 1; for(size_t i=0; i < triangleMesh->coordIndex.size(); ++i){ SFVec3f point[3]; bool notside = true; int center = -1; for(int j=0; j < 3; ++j){ point[j] = triangleMesh->coord->point[triangleMesh->coordIndex[i++]]; if(j){ if(point[0][1] == point[j][1]){ notside &= true; } else { notside &= false; } } if(point[j][0] == 0.0 && point[j][2] == 0.0){ center = j; } } if(!notside){ //side bool over=false; double s[3] = { 0.0, 0.0, 0.0 }; for(int j=0; j<3; j++){ double angle = calcangle(point[j]); s[j] = angle / 2.0 /PI; if(s[j] > 0.5) over = true; } for(int j=0; j < 3; ++j){ if(over && s[j]<1.0e-6){ s[j] = 1.0; } texPoint[0] = s[j]; if(point[j][1] > 0.0){ texPoint[1] = 1.0; } else { texPoint[1] = 0.0; } int k = findPoint(triangleMesh->texCoord->point, texPoint); if(k != -1){ triangleMesh->texCoordIndex.push_back(k); }else{ triangleMesh->texCoord->point.push_back(texPoint); triangleMesh->texCoordIndex.push_back(texIndex++); } } triangleMesh->texCoordIndex.push_back(-1); }else{ // top / bottom for(int j=0; j < 3; ++j){ if(j!=center){ double angle = atan2(point[j][2], point[j][0]); texPoint[0] = 0.5 + 0.5 * cos(angle); if(point[0][1] > 0.0){ //top texPoint[1] = 0.5 - 0.5 * sin(angle); } else { //bottom texPoint[1] = 0.5 + 0.5 * sin(angle); } int k = findPoint(triangleMesh->texCoord->point, texPoint); if(k != -1){ triangleMesh->texCoordIndex.push_back(k); }else{ triangleMesh->texCoord->point.push_back(texPoint); triangleMesh->texCoordIndex.push_back(texIndex++); } }else{ triangleMesh->texCoordIndex.push_back(0); } } triangleMesh->texCoordIndex.push_back(-1); } } } void TriangleMeshShaper::defaultTextureMappingSphere(VrmlIndexedFaceSet* triangleMesh, double radius) { triangleMesh->texCoord = new VrmlTextureCoordinate(); SFVec2f texPoint; int texIndex = 0; for(size_t i=0; i < triangleMesh->coordIndex.size(); ++i){ SFVec3f point[3]; bool over = false; double s[3] = { 0.0, 0.0, 0.0}; for(int j=0; j<3; j++){ point[j] = triangleMesh->coord->point[triangleMesh->coordIndex[i++]]; double angle = calcangle(point[j]); s[j] = angle / 2.0 /PI; if(s[j] > 0.5) over = true; } for(int j=0; j < 3; ++j){ if(over && s[j] < 1.0e-6){ s[j] = 1.0; } texPoint << s[j], 1.0 - acos(point[j][1] / radius) / PI; int k = findPoint(triangleMesh->texCoord->point, texPoint); if(k != -1){ triangleMesh->texCoordIndex.push_back(k); }else{ triangleMesh->texCoord->point.push_back(texPoint); triangleMesh->texCoordIndex.push_back(texIndex++); } } triangleMesh->texCoordIndex.push_back(-1); } } void TriangleMeshShaper::defaultTextureMappingExtrusion(VrmlIndexedFaceSet* triangleMesh, VrmlExtrusion* extrusion) { int numSpine = extrusion->spine.size(); int numcross = extrusion->crossSection.size(); triangleMesh->texCoord = new VrmlTextureCoordinate(); std::vector s; std::vector t; double slen = 0.0; s.push_back(0); for(size_t i=1; i < extrusion->crossSection.size(); ++i){ double x = extrusion->crossSection[i][0] - extrusion->crossSection[i-1][0]; double z = extrusion->crossSection[i][1] - extrusion->crossSection[i-1][1]; slen += sqrt(x*x + z*z); s.push_back(slen); } double tlen = 0.0; t.push_back(0); for(size_t i=1; i < extrusion->spine.size(); ++i){ double x = extrusion->spine[i][0] - extrusion->spine[i-1][0]; double y = extrusion->spine[i][1] - extrusion->spine[i-1][1]; double z = extrusion->spine[i][2] - extrusion->spine[i-1][2]; tlen += sqrt(x*x + y*y + z*z); t.push_back(tlen); } for(size_t i=0; i < extrusion->spine.size(); ++i){ SFVec2f point; point[1] = t[i] / tlen; for(size_t j=0; j < extrusion->crossSection.size(); ++j){ point[0] = s[j] / slen; triangleMesh->texCoord->point.push_back(point); } } int endofspin = (numSpine - 1) * (numcross - 1) * 2 * 4; triangleMesh->texCoordIndex.resize(endofspin); copy(triangleMesh->coordIndex.begin(), triangleMesh->coordIndex.begin() + endofspin, triangleMesh->texCoordIndex.begin()); int endofbegincap = endofspin; int endofpoint = triangleMesh->texCoord->point.size(); if(extrusion->beginCap){ if(extrusion->endCap){ endofbegincap += (triangleMesh->coordIndex.size() - endofspin) / 2; } else { endofbegincap = triangleMesh->coordIndex.size(); } double xmin, xmax; double zmin, zmax; xmin = xmax = extrusion->crossSection[0][0]; zmin = zmax = extrusion->crossSection[0][1]; for(size_t i=1; i < extrusion->crossSection.size(); ++i){ xmax = std::max(xmax, extrusion->crossSection[i][0]); xmin = std::min(xmin, extrusion->crossSection[i][0]); zmax = std::max(zmax, extrusion->crossSection[i][1]); zmin = std::min(xmin, extrusion->crossSection[i][1]); } double xsize = xmax - xmin; double zsize = zmax - zmin; for(int i=0; i < numcross; ++i){ SFVec2f point; point[0] = (extrusion->crossSection[i][0] - xmin) / xsize; point[1] = (extrusion->crossSection[i][1] - zmin) / zsize; triangleMesh->texCoord->point.push_back(point); } for(int i=endofspin; i < endofbegincap; ++i){ int k=triangleMesh->coordIndex[i]; if(k != -1){ triangleMesh->texCoordIndex.push_back(k+endofpoint); } else { triangleMesh->texCoordIndex.push_back(-1); } } } if(extrusion->endCap){ double xmax, xmin; double zmax, zmin; xmin = xmax = extrusion->crossSection[0][0]; zmin = zmax = extrusion->crossSection[0][1]; for(size_t i=1; i < extrusion->crossSection.size(); ++i){ xmax = std::max(xmax, extrusion->crossSection[i][0]); xmin = std::min(xmin, extrusion->crossSection[i][0]); zmax = std::max(zmax, extrusion->crossSection[i][1]); zmin = std::min(xmin, extrusion->crossSection[i][1]); } double xsize = xmax - xmin; double zsize = zmax - zmin; for(size_t i=0; i < extrusion->crossSection.size(); ++i){ SFVec2f point; point[0] = (extrusion->crossSection[i][0] - xmin) / xsize; point[1] = (extrusion->crossSection[i][1] - zmin) / zsize; triangleMesh->texCoord->point.push_back(point); } for(size_t i=endofbegincap; i < triangleMesh->coordIndex.size(); ++i){ int k=triangleMesh->coordIndex[i]; if(k != -1){ triangleMesh->texCoordIndex.push_back(triangleMesh->texCoord->point.size() + k - endofpoint); } else { triangleMesh->texCoordIndex.push_back(-1); } } } } bool TriangleMeshShaper::convertBox(VrmlBox* box, VrmlIndexedFaceSetPtr& triangleMesh) { return impl->convertBox(box, triangleMesh); } choreonoid-1.1.0+dfsg/src/Util/TriangleMeshShaper.h000066400000000000000000000032001207742442300221630ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_TRIANGLE_MESH_SHAPER_H_INCLUDED #define CNOID_UTIL_TRIANGLE_MESH_SHAPER_H_INCLUDED #include "VrmlNodes.h" #include #include "exportdecl.h" namespace cnoid { class TMSImpl; class CNOID_EXPORT TriangleMeshShaper { public: TriangleMeshShaper(); ~TriangleMeshShaper(); void setDivisionNumber(int n); void setNormalGenerationMode(bool on); VrmlNodePtr apply(VrmlNodePtr topNode); SFNode getOriginalGeometry(VrmlShapePtr shapeNode); void defaultTextureMapping(VrmlShape* shapeNode); boost::signal sigMessage; bool convertBox(VrmlBox* box, VrmlIndexedFaceSetPtr& triangleMesh); private: TMSImpl* impl; void defaultTextureMappingFaceSet(VrmlIndexedFaceSet* triangleMesh); void defaultTextureMappingElevationGrid(VrmlElevationGrid* grid, VrmlIndexedFaceSet* triangleMesh); void defaultTextureMappingBox(VrmlIndexedFaceSet* triangleMesh); void defaultTextureMappingCone(VrmlIndexedFaceSet* triangleMesh); void defaultTextureMappingCylinder(VrmlIndexedFaceSet* triangleMesh); void defaultTextureMappingSphere(VrmlIndexedFaceSet* triangleMesh, double radius); void defaultTextureMappingExtrusion(VrmlIndexedFaceSet* triangleMesh, VrmlExtrusion* extrusion ); int faceofBox(SFVec3f* point); int findPoint(MFVec2f& points, SFVec2f& target); double calcangle(SFVec3f& point); }; enum { LEFT, TOP, FRONT, BOTTOM, RIGHT, BACK }; }; #endif choreonoid-1.1.0+dfsg/src/Util/Triangulator.cpp000066400000000000000000000065061207742442300214600ustar00rootroot00000000000000/*! @author Shin'ichiro Nakaoka */ #include "Triangulator.h" using namespace std; using namespace cnoid; int Triangulator::apply(const vector& polygon) { triangles_.clear(); size_t numOrgVertices = polygon.size(); if(numOrgVertices > earMask.size()){ earMask.resize(numOrgVertices); } if(numOrgVertices < 3){ return 0; } else if(numOrgVertices == 3){ triangles_.push_back(0); triangles_.push_back(1); triangles_.push_back(2); return 1; } orgPolygon = &polygon; workPolygon.resize(numOrgVertices); for(size_t i=0; i < numOrgVertices; ++i){ workPolygon[i] = i; } ccs.setZero(); const SFVec3f& o = vertex(0); for(size_t i=1; i < numOrgVertices - 1; ++i){ ccs += (vertex(i) - o).cross(vertex((i+1) % numOrgVertices) - o); } int numTriangles = 0; while(true) { int n = workPolygon.size(); if(n < 3){ break; } int target = -1; for(int i=0; i < n; ++i){ Convexity convexity = calcConvexity(i); if(convexity == FLAT){ target = i; break; } else if(convexity == CONVEX){ if(!checkIfEarContainsOtherVertices(i)){ triangles_.push_back(workPolygon[(i + n - 1) % n]); triangles_.push_back(workPolygon[i]); triangles_.push_back(workPolygon[(i + 1) % n]); target = i; numTriangles++; break; } } } if(target < 0){ break; } for(int i = target + 1; i < n; ++i){ workPolygon[target++] = workPolygon[i]; } workPolygon.pop_back(); } return numTriangles; } Triangulator::Convexity Triangulator::calcConvexity(int ear) { int n = workPolygon.size(); const SFVec3f& p0 = workVertex((ear + n - 1) % n); SFVec3f a = workVertex(ear) - p0; SFVec3f b = workVertex((ear + 1) % n) - p0; SFVec3f ccs = a.cross(b); Convexity convexity; if((ccs.norm() / (a.norm() + b.norm())) < 1.0e-4){ convexity = FLAT; } else { convexity = (this->ccs.dot(ccs) > 0.0) ? CONVEX : CONCAVE; } return convexity; } bool Triangulator::checkIfEarContainsOtherVertices(int ear) { bool contains = false; const int n = workPolygon.size(); if(n > 3){ const int prev = (ear + n -1) % n; const int next = (ear+1) % n; const SFVec3f& a = workVertex(prev); const SFVec3f& b = workVertex(ear); const SFVec3f& c = workVertex(next); earMask[prev] = earMask[ear] = earMask[next] = 1; for(size_t i=0; i < workPolygon.size(); ++i){ if(!earMask[i]){ const SFVec3f& p = workVertex(i); if(((a - p).cross(b - p)).dot(ccs) <= 0){ continue; } if(((b - p).cross(c - p)).dot(ccs) <= 0){ continue; } if(((c - p).cross(a - p)).dot(ccs) <= 0){ continue; } contains = true; break; } } earMask[prev] = earMask[ear] = earMask[next] = 0; } return contains; } choreonoid-1.1.0+dfsg/src/Util/Triangulator.h000066400000000000000000000030361207742442300211200ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_TRIANGULATOR_H_INCLUDED #define CNOID_UTIL_TRIANGULATOR_H_INCLUDED #include "VrmlNodes.h" #include namespace cnoid { class Triangulator { public: inline void setVertices(const MFVec3f& vertices) { this->vertices = &vertices; } /** @return The number of triangles */ int apply(const std::vector& polygon); /** Triangulated indices. This value is available after calling the 'apply' method. The indices are local ones in the polygon index vector given to the apply method. */ inline const std::vector& triangles() { return triangles_; } private: enum Convexity { FLAT, CONVEX, CONCAVE }; const MFVec3f* vertices; const std::vector* orgPolygon; std::vector triangles_; std::vector workPolygon; SFVec3f ccs; // cyclic cross sum boost::dynamic_bitset<> earMask; inline const SFVec3f& vertex(int localIndex){ return (*vertices)[(*orgPolygon)[localIndex]]; } inline const SFVec3f& workVertex(int workPolygonIndex){ return (*vertices)[(*orgPolygon)[workPolygon[workPolygonIndex]]]; } Convexity calcConvexity(int ear); bool checkIfEarContainsOtherVertices(int ear); }; } #endif choreonoid-1.1.0+dfsg/src/Util/Utf8.cpp000066400000000000000000000060531207742442300176300ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "Utf8.h" #include #ifdef _WIN32 #include #include #endif namespace cnoid { #ifdef _MSC_VER const std::string fromUtf8(const std::string& text) { int size = ::MultiByteToWideChar(CP_UTF8, 0, text.c_str(), text.size(), NULL, 0); if(size >= 0){ std::vector wtext(size + 1); ::MultiByteToWideChar(CP_UTF8, 0, text.c_str(), text.size(), &wtext[0], size + 1); int codepage = _getmbcp(); size = ::WideCharToMultiByte(codepage, 0, &wtext[0], size, NULL, 0, NULL, NULL); if(size >= 0){ std::vector converted(size + 1); ::WideCharToMultiByte(codepage, 0, &wtext[0], size, &converted[0], size + 1, NULL, NULL); return std::string(&converted[0], size); } } return text; } const std::string fromUtf8(const char* text) { int size = ::MultiByteToWideChar(CP_UTF8, 0, text, -1, NULL, 0); if(size >= 0){ std::vector wtext(size + 1); ::MultiByteToWideChar(CP_UTF8, 0, text, -1, &wtext[0], size + 1); int codepage = _getmbcp(); size = ::WideCharToMultiByte(codepage, 0, &wtext[0], size, NULL, 0, NULL, NULL); if(size >= 0){ std::vector converted(size + 1); ::WideCharToMultiByte(codepage, 0, &wtext[0], size, &converted[0], size + 1, NULL, NULL); return std::string(&converted[0], size); } } return text; } const std::string toUtf8(const std::string& text) { int codepage = _getmbcp(); int size = ::MultiByteToWideChar(codepage, 0, text.c_str(), text.size(), NULL, 0); if(size >= 0){ std::vector wtext(size + 1); ::MultiByteToWideChar(codepage, 0, text.c_str(), text.size(), &wtext[0], size + 1); size = ::WideCharToMultiByte(CP_UTF8, 0, &wtext[0], size, NULL, 0, NULL, NULL); if(size >= 0){ std::vector converted(size + 1); ::WideCharToMultiByte(CP_UTF8, 0, &wtext[0], size, &converted[0], size + 1, NULL, NULL); return std::string(&converted[0], size); } } return text; } const std::string toUtf8(const char* text) { int codepage = _getmbcp(); int size = ::MultiByteToWideChar(codepage, 0, text, -1, NULL, 0); if(size >= 0){ std::vector wtext(size + 1); ::MultiByteToWideChar(codepage, 0, text, -1, &wtext[0], size + 1); size = ::WideCharToMultiByte(CP_UTF8, 0, &wtext[0], size, NULL, 0, NULL, NULL); if(size >= 0){ std::vector converted(size + 1); ::WideCharToMultiByte(CP_UTF8, 0, &wtext[0], size, &converted[0], size + 1, NULL, NULL); return std::string(&converted[0], size); } } return text; } #endif } choreonoid-1.1.0+dfsg/src/Util/Utf8.h000066400000000000000000000016431207742442300172750ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_UTF8_H_INCLUDED #define CNOID_UTIL_UTF8_H_INCLUDED #include #include "exportdecl.h" namespace cnoid { #ifdef _WIN32 CNOID_EXPORT const std::string toUtf8(const std::string& text); CNOID_EXPORT const std::string fromUtf8(const std::string& text); CNOID_EXPORT const std::string toUtf8(const char* text); CNOID_EXPORT const std::string fromUtf8(const char* text); #else inline const std::string& toUtf8(const std::string& text) { return text; } inline const std::string& fromUtf8(const std::string& text) { return text; } inline const std::string toUtf8(const char* text) { return text; } inline const std::string fromUtf8(const char* text) { return text; } //inline std::string toUtf8(const std::string& text) { return text; } //inline std::string fromUtf8(const std::string& text) { return text; } #endif } #endif choreonoid-1.1.0+dfsg/src/Util/Vector3Seq.cpp000066400000000000000000000055121207742442300207770ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #include "Vector3Seq.h" #include "PlainSeqFormatLoader.h" #include "YamlNodes.h" #include "YamlWriter.h" #include using namespace std; using namespace boost; using namespace cnoid; Vector3Seq::Vector3Seq(int nFrames, double frameRate) : BaseSeqType("Vector3Seq", nFrames, frameRate) { } Vector3Seq::Vector3Seq(const Vector3Seq& org) : BaseSeqType(org) { } Vector3Seq::~Vector3Seq() { } bool Vector3Seq::loadPlainFormat(const std::string& filename) { PlainSeqFileLoader loader; if(!loader.load(filename)){ setIoErrorMessage(loader.errorMessage()); return false; } if(loader.numParts() < 3){ setIoErrorMessage(filename + "does not have 3 columns for 3d vector elements"); return false; } setNumFrames(loader.numFrames()); setFrameRate(1.0 / loader.timeStep()); int frame = 0; for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){ vector& data = *it; (*this)[frame++] << data[1], data[2], data[3]; } return true; } bool Vector3Seq::saveAsPlainFormat(const std::string& filename) { ofstream os(filename.c_str()); os.setf(ios::fixed); if(!os){ setIoErrorMessage(filename + " cannot be opened."); return false; } static format f("%1$.4f %2$.6f %3$.6f %4$.6f\n"); const int n = numFrames(); const double r = frameRate(); for(int i=0; i < n; ++i){ const Vector3& v = (*this)[i]; os << (f % (i / r) % v.x() % v.y() % v.z()); } return true; } bool Vector3Seq::write(YamlWriter& writer) { bool result = false; writer.startMapping(); if(BaseSeqType::write(writer)){ writer.putKey("frames"); writer.startSequence(); const int n = numFrames(); for(int i=0; i < n; ++i){ writer.startFlowStyleSequence(); const Vector3& v = (*this)[i]; for(int j=0; j < 3; ++j){ writer.putScalar(v[j]); } writer.endSequence(); } writer.endSequence(); result = true; } writer.endMapping(); return result; } bool Vector3Seq::read(const YamlMapping& archive) { if(BaseSeqType::read(archive) && (archive["type"].toString() == seqType())){ const YamlSequence& frames = *archive.findSequence("frames"); if(frames.isValid()){ const int n = frames.size(); setNumFrames(n); for(int i=0; i < n; ++i){ const YamlSequence& frame = *frames[i].toSequence(); Vector3& v = (*this)[i]; for(int j=0; j < 3; ++j){ v[j] = frame[j].toDouble(); } } } return true; } return false; } choreonoid-1.1.0+dfsg/src/Util/Vector3Seq.h000066400000000000000000000015701207742442300204440ustar00rootroot00000000000000/** @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_VECTOR3_SEQ_H_INCLUDED #define CNOID_UTIL_VECTOR3_SEQ_H_INCLUDED #include "Seq.h" #include "EigenUtil.h" #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT Vector3Seq : public Seq { typedef Seq BaseSeqType; public: Vector3Seq(int nFrames = 0, double frameRate = 100.0); Vector3Seq(const Vector3Seq& org); virtual ~Vector3Seq(); virtual bool write(YamlWriter& writer); virtual bool read(const YamlMapping& archive); virtual bool loadPlainFormat(const std::string& filename); virtual bool saveAsPlainFormat(const std::string& filename); protected: virtual Vector3 defaultValue() const { return Vector3::Zero(); } }; typedef boost::shared_ptr Vector3SeqPtr; } #endif choreonoid-1.1.0+dfsg/src/Util/VrmlNodes.cpp000066400000000000000000000225111207742442300207100ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "VrmlNodes.h" using namespace cnoid; const char* cnoid::labelOfVrmlFieldTypeId(int type) { switch(type){ case SFINT32: return "SFInt32"; case MFINT32: return "MFInt32"; case SFFLOAT: return "SFFloat"; case MFFLOAT: return "MFFloat"; case SFVEC2F: return "SFVec3f"; case MFVEC2F: return "MFVec2f"; case SFVEC3F: return "SFVec3f"; case MFVEC3F: return "MFVec3f"; case SFROTATION: return "SFRotation"; case MFROTATION: return "MFRotation"; case SFTIME: return "SFTime"; case MFTIME: return "MFTime"; case SFCOLOR: return "SFColor"; case MFCOLOR: return "MFColor"; case SFSTRING: return "SFString"; case MFSTRING: return "MFString"; case SFNODE: return "SFNode"; case MFNODE: return "MFNode"; case SFBOOL: return "SFBool"; case SFIMAGE: return "SFImage"; default: return "Unknown Field Type"; break; } } VrmlNode::VrmlNode() { refCounter = 0; } VrmlNode::~VrmlNode() { } bool VrmlNode::isCategoryOf(VrmlNodeCategory category) { return (category == ANY_NODE) ? true : categorySet.test(category); } VrmlUnsupportedNode::VrmlUnsupportedNode(const std::string& nodeTypeName) : nodeTypeName(nodeTypeName) { categorySet.set(TOP_NODE); categorySet.set(CHILD_NODE); } VrmlViewpoint::VrmlViewpoint() { categorySet.set(TOP_NODE); categorySet.set(BINDABLE_NODE); categorySet.set(CHILD_NODE); fieldOfView = 0.785398; jump = true; orientation.axis() = SFVec3f::UnitZ(); orientation.angle() = 0.0; position << 0.0, 0.0, 10.0; } VrmlNavigationInfo::VrmlNavigationInfo() : avatarSize(3) { categorySet.set(TOP_NODE); categorySet.set(BINDABLE_NODE); categorySet.set(CHILD_NODE); avatarSize[0] = 0.25; avatarSize[1] = 1.6; avatarSize[2] = 0.75; headlight = true; speed = 1.0; visibilityLimit = 0.0; type.push_back("WALK"); } VrmlBackground::VrmlBackground() { categorySet.set(TOP_NODE); categorySet.set(BINDABLE_NODE); categorySet.set(CHILD_NODE); } AbstractVrmlGroup::AbstractVrmlGroup() { categorySet.set(TOP_NODE); categorySet.set(GROUPING_NODE); categorySet.set(CHILD_NODE); } void AbstractVrmlGroup::removeChild(int childIndex) { replaceChild(childIndex, 0); } VrmlGroup::VrmlGroup() { bboxCenter.setZero(); bboxSize.fill(-1.0); } MFNode& VrmlGroup::getChildren() { return children; } int VrmlGroup::countChildren() { return children.size(); } VrmlNode* VrmlGroup::getChild(int index) { return children[index].get(); } void VrmlGroup::replaceChild(int childIndex, VrmlNode* childNode) { if(!childNode){ children.erase(children.begin() + childIndex); } else { children[childIndex] = childNode; } } VrmlTransform::VrmlTransform() { center.setZero(); rotation.axis() = SFVec3f::UnitZ(); rotation.angle() = 0.0; scale.setOnes(); scaleOrientation.axis() = SFVec3f::UnitZ(); scaleOrientation.angle() = 0.0; translation.setZero(); } Eigen::Affine3d VrmlTransform::toAffine3d() { const Eigen::Translation3d C(center); const SFRotation& SR = scaleOrientation; const Eigen::AlignedScaling3d S(scale); const SFRotation& R = rotation; const Eigen::Translation3d T(translation); return T * C * R * SR * S * SR.inverse() * C.inverse(); } VrmlInline::VrmlInline() { categorySet.set(INLINE_NODE); } VrmlShape::VrmlShape() { categorySet.set(TOP_NODE); categorySet.set(CHILD_NODE); categorySet.set(SHAPE_NODE); } VrmlAppearance::VrmlAppearance() { categorySet.set(APPEARANCE_NODE); } VrmlMaterial::VrmlMaterial() { categorySet.set(MATERIAL_NODE); diffuseColor << 0.0f, 0.0f, 0.8f; emissiveColor.setZero(); specularColor.setZero(); ambientIntensity = 0.2; shininess = 0.2; transparency = 0.0; } VrmlTexture::VrmlTexture() { categorySet.set(TEXTURE_NODE); } VrmlImageTexture::VrmlImageTexture() { repeatS = true; repeatT = true; } VrmlTextureTransform::VrmlTextureTransform() { categorySet.set(TEXTURE_TRANSFORM_NODE); center.setZero(); scale.setOnes(); translation.setZero(); rotation = 0.0; } VrmlGeometry::VrmlGeometry() { categorySet.set(GEOMETRY_NODE); } VrmlBox::VrmlBox() { size.fill(2.0); } VrmlCone::VrmlCone() { bottom = true; bottomRadius = 1.0; height = 2.0; side = true; } VrmlCylinder::VrmlCylinder() { height = 2.0; radius = 1.0; bottom = true; side = true; top = true; } VrmlSphere::VrmlSphere() { radius = 1.0; } VrmlFontStyle::VrmlFontStyle() { categorySet.set(FONT_STYLE_NODE); family.push_back("SERIF"); horizontal = true; justify.push_back("BEGIN"); leftToRight = true; size = 1.0; spacing = 1.0; style = "PLAIN"; topToBottom = true; } VrmlText::VrmlText() { maxExtent = 0.0; } VrmlIndexedLineSet::VrmlIndexedLineSet() { colorPerVertex = true; } VrmlIndexedFaceSet::VrmlIndexedFaceSet() { ccw = true; convex = true; creaseAngle = 0.0; normalPerVertex = true; solid = true; } VrmlColor::VrmlColor() { categorySet.set(COLOR_NODE); } VrmlCoordinate::VrmlCoordinate() { categorySet.set(COORDINATE_NODE); } VrmlTextureCoordinate::VrmlTextureCoordinate() { categorySet.set(TEXTURE_COORDINATE_NODE); } VrmlNormal::VrmlNormal() { categorySet.set(NORMAL_NODE); } VrmlCylinderSensor::VrmlCylinderSensor() { categorySet.set(CHILD_NODE); categorySet.set(SENSOR_NODE); autoOffset = true; diskAngle = 0.262; enabled = true; maxAngle = -1; minAngle = 0; offset = 0; } VrmlPointSet::VrmlPointSet() { coord = NULL; color = NULL; } VrmlPixelTexture::VrmlPixelTexture() { image.width = 0; image.height = 0; image.numComponents = 0; image.pixels.clear(); repeatS = true; repeatT = true; } VrmlMovieTexture::VrmlMovieTexture() { // url loop = false; speed = 0; startTime = 0.0; stopTime = 0.0; repeatS = true; repeatT = true; } VrmlElevationGrid::VrmlElevationGrid() { xDimension = 0; zDimension = 0; xSpacing = 0.0; zSpacing = 0.0; // height // MFFloat ccw = true; colorPerVertex = true; creaseAngle = 0.0; normalPerVertex = true; solid = true; color = NULL; normal = NULL; texCoord = NULL; } VrmlExtrusion::VrmlExtrusion() { // crossSection // spine beginCap = true; endCap = true; solid = true; ccw = true; convex = true; creaseAngle = 0; orientation.push_back(SFRotation(0.0, SFVec3f::UnitZ())); scale.push_back(SFVec2f(1.0, 1.0)); } VrmlSwitch::VrmlSwitch() { whichChoice = -1; } MFNode& VrmlSwitch::getChildren() { return choice; } int VrmlSwitch::countChildren() { return choice.size(); } VrmlNode* VrmlSwitch::getChild(int index) { return choice[index].get(); } void VrmlSwitch::replaceChild(int childIndex, VrmlNode* childNode) { if(!childNode){ choice.erase(choice.begin() + childIndex); if(whichChoice == childIndex){ whichChoice = -1; } else if(whichChoice > childIndex){ whichChoice -= 1; } } else { choice[childIndex] = childNode; } } VrmlLOD::VrmlLOD() { center.setZero(); } MFNode& VrmlLOD::getChildren() { return level; } int VrmlLOD::countChildren() { return level.size(); } VrmlNode* VrmlLOD::getChild(int index) { return level[index].get(); } void VrmlLOD::replaceChild(int childIndex, VrmlNode* childNode) { if(!childNode){ level.erase(level.begin() + childIndex); if(!level.empty()){ int rangeIndexToRemove = (childIndex > 0) ? (childIndex - 1) : 0; range.erase(range.begin() + rangeIndexToRemove); } } else { level[childIndex] = childNode; } } VrmlCollision::VrmlCollision() { collide = true; } VrmlAnchor::VrmlAnchor() { } VrmlBillboard::VrmlBillboard() { axisOfRotation.setZero(); } VrmlFog::VrmlFog() { color.setZero(); visibilityRange = 0.0; fogType = "LINEAR"; } VrmlWorldInfo::VrmlWorldInfo() { categorySet.set(TOP_NODE); } VrmlPointLight::VrmlPointLight() { categorySet.set(TOP_NODE); categorySet.set(CHILD_NODE); location.setZero(); on = true; intensity = 1.0; color.setOnes(); radius = 100.0; ambientIntensity = 0.0; attenuation << 1.0, 0.0, 0.0; } VrmlDirectionalLight::VrmlDirectionalLight() { categorySet.set(TOP_NODE); categorySet.set(CHILD_NODE); ambientIntensity = 0.0; color.setOnes(); direction << 0.0, 0.0, -1.0; intensity = 1.0; on = true; } VrmlSpotLight::VrmlSpotLight() { categorySet.set(TOP_NODE); categorySet.set(CHILD_NODE); location.setZero(); direction << 0.0, 0.0, -1.0; on = true; color.setOnes(); intensity = 1.0; radius = 100.0; ambientIntensity = 0.0; attenuation << 1.0, 0.0, 0.0; beamWidth = 1.570796; cutOffAngle = 0.785398; } VrmlProto::VrmlProto(const std::string& n) : protoName(n) { categorySet.set(TOP_NODE); categorySet.set(PROTO_DEF_NODE); } VrmlProtoInstance::VrmlProtoInstance(VrmlProtoPtr proto0) : proto(proto0), fields(proto0->fields) { categorySet.set(TOP_NODE); categorySet.set(PROTO_INSTANCE_NODE);; categorySet.set(CHILD_NODE); } choreonoid-1.1.0+dfsg/src/Util/VrmlNodes.h000066400000000000000000000477251207742442300203730ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_VRMLNODES_H_INCLUDED #define CNOID_UTIL_VRMLNODES_H_INCLUDED #include #include #include #include #include #include #include #include #include "exportdecl.h" namespace cnoid { typedef bool SFBool; typedef int SFInt32; typedef double SFFloat; typedef std::string SFString; // Define SFTime as struct to clearly distinguish its type from SFFloat struct SFTime { double value; inline SFTime() { value = 0.0; } inline SFTime(double time) { value = time; } inline double operator=(double time) { return (value = time); } }; typedef Eigen::Vector2d SFVec2f; typedef Eigen::Vector3d SFVec3f; typedef Eigen::Array3f SFColor; typedef Eigen::AngleAxisd SFRotation; typedef struct { int width; int height; int numComponents; std::vector pixels; } SFImage; typedef std::vector MFInt32; typedef std::vector MFFloat; typedef std::vector > MFVec2f; typedef std::vector MFVec3f; typedef std::vector MFRotation; typedef std::vector MFTime; typedef std::vector MFColor; typedef std::vector MFString; // see 4.6.3 - 4.6.10 of the VRML97 specification enum VrmlNodeCategory { ANY_NODE = -1, PROTO_DEF_NODE = 0, PROTO_INSTANCE_NODE, TOP_NODE, BINDABLE_NODE, GROUPING_NODE, CHILD_NODE, APPEARANCE_NODE, MATERIAL_NODE, TEXTURE_NODE, TEXTURE_TRANSFORM_NODE, SHAPE_NODE, GEOMETRY_NODE, COORDINATE_NODE, COLOR_NODE, NORMAL_NODE, TEXTURE_COORDINATE_NODE, FONT_STYLE_NODE, SENSOR_NODE, INLINE_NODE, NUM_VRML_NODE_CATEGORIES }; class VrmlNode; inline void intrusive_ptr_add_ref(VrmlNode* obj); inline void intrusive_ptr_release(VrmlNode* obj); //! Abstract base class of all vrml nodes. class CNOID_EXPORT VrmlNode { public: VrmlNode(); virtual ~VrmlNode(); std::string defName; bool isCategoryOf(VrmlNodeCategory category); protected: std::bitset categorySet; private: int refCounter; friend void intrusive_ptr_add_ref(VrmlNode* obj); friend void intrusive_ptr_release(VrmlNode* obj); }; inline void intrusive_ptr_add_ref(VrmlNode* obj){ obj->refCounter++; } inline void intrusive_ptr_release(VrmlNode* obj){ obj->refCounter--; if(obj->refCounter <= 0){ delete obj; } } typedef boost::intrusive_ptr VrmlNodePtr; typedef VrmlNodePtr SFNode; typedef std::vector MFNode; class CNOID_EXPORT VrmlUnsupportedNode : public VrmlNode { public: VrmlUnsupportedNode(const std::string& nodeTypeName); std::string nodeTypeName; }; typedef boost::intrusive_ptr VrmlUnsupportedNodePtr; //! VRML Viewpoint node class CNOID_EXPORT VrmlViewpoint : public VrmlNode { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VrmlViewpoint(); SFRotation orientation; SFFloat fieldOfView; SFBool jump; SFVec3f position; SFString description; }; typedef boost::intrusive_ptr VrmlViewpointPtr; //! VRML NavigationInfo node class CNOID_EXPORT VrmlNavigationInfo : public VrmlNode { public: VrmlNavigationInfo(); MFFloat avatarSize; SFBool headlight; SFFloat speed; MFString type; SFFloat visibilityLimit; }; typedef boost::intrusive_ptr VrmlNavigationInfoPtr; //! VRML Background node class CNOID_EXPORT VrmlBackground : public VrmlNode { public: VrmlBackground(); MFFloat groundAngle; MFColor groundColor; MFFloat skyAngle; MFColor skyColor; MFString backUrl; MFString bottomUrl; MFString frontUrl; MFString leftUrl; MFString rightUrl; MFString topUrl; }; typedef boost::intrusive_ptr VrmlBackgroundPtr; class CNOID_EXPORT AbstractVrmlGroup : public VrmlNode { public: AbstractVrmlGroup(); virtual MFNode& getChildren() = 0; virtual int countChildren() = 0; virtual VrmlNode* getChild(int index) = 0; virtual void replaceChild(int childIndex, VrmlNode* childNode) = 0; void removeChild(int childIndex); }; typedef boost::intrusive_ptr AbstractVrmlGroupPtr; //! VRML Group node class CNOID_EXPORT VrmlGroup : public AbstractVrmlGroup { public: VrmlGroup(); virtual MFNode& getChildren(); virtual int countChildren(); virtual VrmlNode* getChild(int index); virtual void replaceChild(int childIndex, VrmlNode* childNode); SFVec3f bboxCenter; SFVec3f bboxSize; MFNode children; }; typedef boost::intrusive_ptr VrmlGroupPtr; //! VRML Transform node class CNOID_EXPORT VrmlTransform : public VrmlGroup { public: VrmlTransform(); Eigen::Affine3d toAffine3d(); SFVec3f center; SFRotation rotation; SFVec3f scale; SFRotation scaleOrientation; SFVec3f translation; }; typedef boost::intrusive_ptr VrmlTransformPtr; //! VRML Inline node class CNOID_EXPORT VrmlInline : public VrmlGroup { public: VrmlInline(); MFString urls; }; typedef boost::intrusive_ptr VrmlInlinePtr; class VrmlAppearance; typedef boost::intrusive_ptr VrmlAppearancePtr; class VrmlGeometry; typedef boost::intrusive_ptr VrmlGeometryPtr; //! VRML Shape node class CNOID_EXPORT VrmlShape : public VrmlNode { public: VrmlShape(); VrmlAppearancePtr appearance; SFNode geometry; }; typedef boost::intrusive_ptr VrmlShapePtr; class VrmlMaterial; typedef boost::intrusive_ptr VrmlMaterialPtr; class VrmlTexture; typedef boost::intrusive_ptr VrmlTexturePtr; class VrmlTextureTransform; typedef boost::intrusive_ptr VrmlTextureTransformPtr; //! VRML Appearance node class CNOID_EXPORT VrmlAppearance : public VrmlNode { public: VrmlAppearance(); VrmlMaterialPtr material; VrmlTexturePtr texture; VrmlTextureTransformPtr textureTransform; }; //! VRML Material node class CNOID_EXPORT VrmlMaterial : public VrmlNode { public: VrmlMaterial(); SFFloat ambientIntensity; SFColor diffuseColor; SFColor emissiveColor; SFFloat shininess; SFColor specularColor; SFFloat transparency; }; //! Base class of VRML Texture nodes class CNOID_EXPORT VrmlTexture : public VrmlNode { public: VrmlTexture(); }; //! VRML ImageTexture node class CNOID_EXPORT VrmlImageTexture : public VrmlTexture { public: VrmlImageTexture(); MFString url; SFBool repeatS; SFBool repeatT; }; typedef boost::intrusive_ptr VrmlImageTexturePtr; //! VRML TextureTransform node class CNOID_EXPORT VrmlTextureTransform : public VrmlNode { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VrmlTextureTransform(); SFVec2f center; SFFloat rotation; SFVec2f scale; SFVec2f translation; }; //! Base class of VRML geometry nodes class CNOID_EXPORT VrmlGeometry : public VrmlNode { public: VrmlGeometry(); }; //! VRML Box node class CNOID_EXPORT VrmlBox : public VrmlGeometry { public: VrmlBox(); SFVec3f size; }; typedef boost::intrusive_ptr VrmlBoxPtr; //! VRML Cone node class CNOID_EXPORT VrmlCone : public VrmlGeometry { public: VrmlCone(); SFBool bottom; SFFloat bottomRadius; SFFloat height; SFBool side; }; typedef boost::intrusive_ptr VrmlConePtr; //! VRML Cylinder node class CNOID_EXPORT VrmlCylinder : public VrmlGeometry { public: VrmlCylinder(); SFBool bottom; SFFloat height; SFFloat radius; SFBool side; SFBool top; }; typedef boost::intrusive_ptr VrmlCylinderPtr; //! VRML Sphere node class CNOID_EXPORT VrmlSphere : public VrmlGeometry { public: VrmlSphere(); SFFloat radius; }; typedef boost::intrusive_ptr VrmlSpherePtr; //! VRML FontStyle node class CNOID_EXPORT VrmlFontStyle : public VrmlNode { public: VrmlFontStyle(); MFString family; SFBool horizontal; MFString justify; SFString language; SFBool leftToRight; SFFloat size; SFFloat spacing; SFString style; SFBool topToBottom; }; typedef boost::intrusive_ptr VrmlFontStylePtr; //! VRML Text node class CNOID_EXPORT VrmlText : public VrmlGeometry { public: VrmlText(); MFString fstring; VrmlFontStylePtr fontStyle; MFFloat length; SFFloat maxExtent; }; typedef boost::intrusive_ptr VrmlTextPtr; class VrmlColor; typedef boost::intrusive_ptr VrmlColorPtr; class VrmlCoordinate; typedef boost::intrusive_ptr VrmlCoordinatePtr; //! VRML IndexedLineSet node class CNOID_EXPORT VrmlIndexedLineSet : public VrmlGeometry { public: VrmlIndexedLineSet(); VrmlColorPtr color; VrmlCoordinatePtr coord; MFInt32 colorIndex; SFBool colorPerVertex; MFInt32 coordIndex; }; typedef boost::intrusive_ptr VrmlIndexedLineSetPtr; class VrmlNormal; typedef boost::intrusive_ptr VrmlNormalPtr; class VrmlTextureCoordinate; typedef boost::intrusive_ptr VrmlTextureCoordinatePtr; //! VRML IndexedFaseSet node class CNOID_EXPORT VrmlIndexedFaceSet : public VrmlIndexedLineSet { public: VrmlIndexedFaceSet(); VrmlNormalPtr normal; VrmlTextureCoordinatePtr texCoord; SFBool ccw; SFBool convex; SFFloat creaseAngle; MFInt32 normalIndex; SFBool normalPerVertex; SFBool solid; MFInt32 texCoordIndex; }; typedef boost::intrusive_ptr VrmlIndexedFaceSetPtr; //! VRML Color node class CNOID_EXPORT VrmlColor : public VrmlNode { public: VrmlColor(); MFColor color; }; //! VRML Coordinate node class CNOID_EXPORT VrmlCoordinate : public VrmlNode { public: VrmlCoordinate(); MFVec3f point; }; //! VRML TextureCoordinate node class CNOID_EXPORT VrmlTextureCoordinate : public VrmlNode { public: VrmlTextureCoordinate(); MFVec2f point; }; //! VRML Normal node class CNOID_EXPORT VrmlNormal : public VrmlNode { public: VrmlNormal(); MFVec3f vector; }; //! VRML CylinderSensor node class CNOID_EXPORT VrmlCylinderSensor : public VrmlNode { public: VrmlCylinderSensor(); SFBool autoOffset; SFFloat diskAngle; SFBool enabled; SFFloat maxAngle; SFFloat minAngle; SFFloat offset; }; typedef boost::intrusive_ptr VrmlCylinderSensorPtr; //! VRML PointSet node class CNOID_EXPORT VrmlPointSet : public VrmlGeometry { public: VrmlPointSet(); VrmlCoordinatePtr coord; VrmlColorPtr color; }; typedef boost::intrusive_ptr VrmlPointSetPtr; //! VRML PixelTexture node class CNOID_EXPORT VrmlPixelTexture : public VrmlTexture { public: VrmlPixelTexture(); SFImage image; SFBool repeatS; SFBool repeatT; }; typedef boost::intrusive_ptr VrmlPixelTexturePtr; //! VRML MovieTexture node class CNOID_EXPORT VrmlMovieTexture : public VrmlTexture { public: VrmlMovieTexture(); MFString url; SFBool loop; SFFloat speed; SFTime startTime; SFTime stopTime; SFBool repeatS; SFBool repeatT; }; typedef boost::intrusive_ptr VrmlMovieTexturePtr; //! VRML ElevationGrid node class CNOID_EXPORT VrmlElevationGrid : public VrmlGeometry { public: VrmlElevationGrid(); SFInt32 xDimension; SFInt32 zDimension; SFFloat xSpacing; SFFloat zSpacing; MFFloat height; SFBool ccw; SFBool colorPerVertex; SFFloat creaseAngle; SFBool normalPerVertex; SFBool solid; VrmlColorPtr color; VrmlNormalPtr normal; VrmlTextureCoordinatePtr texCoord; }; typedef boost::intrusive_ptr VrmlElevationGridPtr; //! VRML Extrusion node class CNOID_EXPORT VrmlExtrusion : public VrmlGeometry { public: VrmlExtrusion(); MFVec2f crossSection; MFVec3f spine; MFVec2f scale; MFRotation orientation; SFBool beginCap; SFBool endCap; SFBool solid; SFBool ccw; SFBool convex; SFFloat creaseAngle; }; typedef boost::intrusive_ptr VrmlExtrusionPtr; class CNOID_EXPORT VrmlSwitch : public AbstractVrmlGroup { public: VrmlSwitch(); virtual MFNode& getChildren(); virtual int countChildren(); virtual VrmlNode* getChild(int index); virtual void replaceChild(int childIndex, VrmlNode* childNode); MFNode choice; SFInt32 whichChoice; }; typedef boost::intrusive_ptr VrmlSwitchPtr; class CNOID_EXPORT VrmlLOD : public AbstractVrmlGroup { public: VrmlLOD(); virtual MFNode& getChildren(); virtual int countChildren(); virtual VrmlNode* getChild(int index); virtual void replaceChild(int childIndex, VrmlNode* childNode); MFFloat range; SFVec3f center; MFNode level; }; typedef boost::intrusive_ptr VrmlLODPtr; class CNOID_EXPORT VrmlCollision : public VrmlGroup { public: VrmlCollision(); SFBool collide; SFNode proxy; }; typedef boost::intrusive_ptr VrmlCollisionPtr; class CNOID_EXPORT VrmlAnchor : public VrmlGroup { public: VrmlAnchor(); SFString description; MFString parameter; MFString url; }; typedef boost::intrusive_ptr VrmlAnchorPtr; class CNOID_EXPORT VrmlBillboard : public VrmlGroup { public: VrmlBillboard(); SFVec3f axisOfRotation; }; typedef boost::intrusive_ptr VrmlBillboardPtr; class CNOID_EXPORT VrmlFog : public VrmlNode { public: VrmlFog(); SFColor color; SFFloat visibilityRange; SFString fogType; }; typedef boost::intrusive_ptr VrmlFogPtr; class CNOID_EXPORT VrmlWorldInfo : public VrmlNode { public: VrmlWorldInfo(); SFString title; MFString info; }; typedef boost::intrusive_ptr VrmlWorldInfoPtr; class CNOID_EXPORT VrmlPointLight : public VrmlNode { public: VrmlPointLight(); SFVec3f location; SFBool on; SFFloat intensity; SFColor color; SFFloat radius; SFFloat ambientIntensity; SFVec3f attenuation; }; typedef boost::intrusive_ptr VrmlPointLightPtr; class CNOID_EXPORT VrmlDirectionalLight : public VrmlNode { public: VrmlDirectionalLight(); SFFloat ambientIntensity; SFColor color; SFVec3f direction; SFFloat intensity; SFBool on; }; typedef boost::intrusive_ptr VrmlDirectionalLightPtr; class CNOID_EXPORT VrmlSpotLight : public VrmlNode { public: VrmlSpotLight(); SFVec3f location; SFVec3f direction; SFBool on; SFColor color; SFFloat intensity; SFFloat radius; SFFloat ambientIntensity; SFVec3f attenuation; SFFloat beamWidth; SFFloat cutOffAngle; }; typedef boost::intrusive_ptr VrmlSpotLightPtr; typedef boost::variant VrmlVariantField; enum VrmlFieldTypeId { SFBOOL, SFINT32, SFFLOAT, SFVEC2F, SFVEC3F, SFROTATION, SFCOLOR, SFTIME, SFSTRING, SFNODE, SFIMAGE, MFINT32, MFFLOAT, MFVEC2F, MFVEC3F, MFROTATION, MFCOLOR, MFTIME, MFSTRING, MFNODE, UNKNOWN_VRML_FIELD_TYPE }; typedef std::map TProtoFieldMap; typedef std::pair TProtoFieldPair; template int vrmlFieldTypeId() { return UNKNOWN_VRML_FIELD_TYPE; } template<> inline int vrmlFieldTypeId() { return SFINT32; } template<> inline int vrmlFieldTypeId() { return MFINT32; } template<> inline int vrmlFieldTypeId() { return SFFLOAT; } template<> inline int vrmlFieldTypeId() { return MFFLOAT; } template<> inline int vrmlFieldTypeId() { return SFVEC3F; } template<> inline int vrmlFieldTypeId() { return MFVEC3F; } template<> inline int vrmlFieldTypeId() { return SFROTATION; } template<> inline int vrmlFieldTypeId() { return MFROTATION; } template<> inline int vrmlFieldTypeId() { return SFTIME; } template<> inline int vrmlFieldTypeId() { return MFTIME; } template<> inline int vrmlFieldTypeId() { return SFCOLOR; } template<> inline int vrmlFieldTypeId() { return MFCOLOR; } template<> inline int vrmlFieldTypeId() { return SFSTRING; } template<> inline int vrmlFieldTypeId() { return MFSTRING; } template<> inline int vrmlFieldTypeId() { return SFNODE; } template<> inline int vrmlFieldTypeId() { return MFNODE; } template<> inline int vrmlFieldTypeId() { return SFIMAGE; } CNOID_EXPORT const char* labelOfVrmlFieldTypeId(int type); template inline const char* labelOfVrmlFieldType() { return labelOfVrmlFieldTypeId(vrmlFieldTypeId()); } //! VRML Proto definition class CNOID_EXPORT VrmlProto : public VrmlNode { public: std::string protoName; TProtoFieldMap fields; VrmlProto(const std::string& n); inline VrmlVariantField* findField(const std::string& fieldName) { TProtoFieldMap::iterator p = fields.find(fieldName); return (p != fields.end()) ? &p->second : 0; } inline VrmlVariantField& field(const std::string& fieldName){ return fields[fieldName]; } /* inline VrmlVariantField& addField(const std::string& fieldName, VrmlFieldTypeId typeId){ VrmlVariantField* field = &(fields[fieldName]); field->setType(typeId); return field; } */ }; typedef boost::intrusive_ptr VrmlProtoPtr; //! VRML node which is instance of VRML Prototype class CNOID_EXPORT VrmlProtoInstance : public VrmlNode { public: VrmlProtoPtr proto; TProtoFieldMap fields; VrmlNodePtr actualNode; VrmlProtoInstance(VrmlProtoPtr proto0); inline VrmlVariantField* findField(const std::string& fieldName) { TProtoFieldMap::iterator p = fields.find(fieldName); return (p != fields.end()) ? &p->second : 0; } }; typedef boost::intrusive_ptr VrmlProtoInstancePtr; /** The upper cast operation that supports the situation where the original pointer is VrmlProtoInstance and you want to get the actual node, the node replaced with the pre-defined node type written in the PROTO definition. */ template inline boost::intrusive_ptr dynamic_node_cast(VrmlNodePtr node) { VrmlProtoInstancePtr protoInstance = boost::dynamic_pointer_cast(node); if(protoInstance){ return boost::dynamic_pointer_cast(protoInstance->actualNode); } else { return boost::dynamic_pointer_cast(node); } } }; #endif choreonoid-1.1.0+dfsg/src/Util/VrmlParser.cpp000066400000000000000000002321151207742442300210770ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka @author K.Fukuda @author Takafumi.Tawara */ #include "VrmlParser.h" #include "EasyScanner.h" #include "Utf8.h" #include #include #include #include #if (BOOST_VERSION <= 103301) #include #include #else #include #endif using namespace std; using namespace boost; using namespace cnoid; namespace { /*! @if jp @brief URLスキーム(file:)文字列を削除 @param[in] url 検索・置æ›å¯¾è±¡ã¨ãªã‚‹URL @return string URLスキーム文字列をå–り除ã„ãŸæ–‡å­—列 @endif */ string deleteURLScheme(string url) { static const string fileProtocolHeader1("file://"); static const string fileProtocolHeader2("file:"); size_t pos = url.find( fileProtocolHeader1 ); if( 0 == pos ) { url.erase( 0, fileProtocolHeader1.size() ); } else { size_t pos = url.find( fileProtocolHeader2 ); if( 0 == pos ) { url.erase( 0, fileProtocolHeader2.size() ); } } // Windows ドライブ文字列ã®æ™‚ã¯ãƒ‡ã‚£ãƒ¬ã‚¯ãƒˆãƒªåŒºåˆ‡ã‚Šæ–‡å­—分をã•ã‚‰ã«å‰Šé™¤// if ( url.find(":") == 2 && url.find("/") ==0 ) url.erase ( 0, 1 ); return url; } /*! @if jp @brief URLãŒãƒ•ã‚¡ã‚¤ãƒ«ã‹ã©ã†ã‹ã®åˆ¤å®š @param[in] ref 判定対象ã®URL @return boolean true:ローカルファイルã§ã‚ã‚‹ @endif */ bool isFileProtocol(const string& ref) { bool ret = false; string::size_type pos = ref.find(":"); if ( pos == string::npos || pos == 1 ) { // Directly local path || Windows drive letter separator ret = true; } else { if( ref.find("file:") == 0 ) ret = true; } return ret; } /*! @if jp @brief URLスキーム文字列をå–り除ã„ãŸæ–‡å­—列を生æˆã™ã‚‹ @param[out] refUrl URLスキーム文字列をå–り除ã„ãŸæ–‡å­—åˆ—ã‚’æ ¼ç´ @param[in] rootDir 親ディレクトリ @param[in] srcPath å…ƒã¨ãªã‚‹URL @return URLスキーム文字列をå–り除ã„ãŸæ–‡å­—列 @endif */ void getPathFromUrl(string& refUrl, const string& rootDir, string srcUrl) { if ( isFileProtocol(srcUrl) ){ // ローカルファイル // filesystem::path filepath( deleteURLScheme(srcUrl), filesystem::native); if(filesystem::exists(filepath)){ // å…ƒãŒçµ¶å¯¾ãƒ‘ス // refUrl = filesystem::system_complete(filepath).file_string(); }else{ // å…ƒãŒç›¸å¯¾ãƒ‘ス // filesystem::path filepath(rootDir + deleteURLScheme(srcUrl), filesystem::native); if(filesystem::exists(filepath)){ refUrl = filesystem::system_complete(filepath).file_string(); } } } else { // ファイルスキーム以外ã®å‡¦ç† // } } /** The definition of the reserved word IDs */ enum { NO_SYMBOL = 0, // values V_TRUE, V_FALSE, V_NULL, // types T_SFINT32, T_MFINT32, T_SFFLOAT, T_MFFLOAT, T_SFVEC2F, T_MFVEC2F, T_SFVEC3F, T_MFVEC3F, T_SFSTRING, T_MFSTRING, T_SFROTATION, T_MFROTATION, T_SFTIME, T_MFTIME, T_SFCOLOR, T_MFCOLOR, T_SFNODE, T_MFNODE, T_SFBOOL, T_SFIMAGE, // Nodes N_PROTO, N_INLINE, N_BACKGROUND, N_NAVIGATION_INFO, N_VIEWPOINT, N_GROUP, N_TRANSFORM, N_APPEARANCE, N_MATERIAL, N_IMAGE_TEXTURE, N_TEXTURE_TRANSFORM, N_SHAPE, N_BOX, N_CONE, N_CYLINDER, N_SPHERE, N_TEXT, N_FONT_STYLE, N_INDEXED_LINE_SET, N_INDEXED_FACE_SET, N_COLOR, N_COORDINATE, N_TEXTURE_COORDINATE, N_NORMAL, N_CYLINDER_SENSOR, N_POINTSET, N_PIXEL_TEXTURE, N_MOVIE_TEXTURE, N_ELEVATION_GRID, N_EXTRUSION, N_SWITCH, N_LOD, N_COLLISION, N_ANCHOR, N_FOG, N_BILLBOARD, N_WORLD_INFO, N_POINT_LIGHT, N_DIRECTIONAL_LIGHT, N_SPOT_LIGHT, N_AUDIO_CLIP, N_SOUND, N_COLOR_INTERPOLATOR, N_COORDINATE_INTERPOLATOR, N_ORIENTATION_INTERPOLATOR, N_NORMAL_INTERPOLATOR, N_POSITION_INTERPOLATOR, N_SCALAR_INTERPOLATOR, N_PLANE_SENSOR, N_PROXIMITY_SENSOR, N_SPHERE_SENSOR, N_TIME_SENSOR, N_TOUCH_SENSOR, N_VISIBILITY_SENSOR, // Fields F_IS, F_URL, F_GROUND_ANGLE, F_GROUND_COLOR, F_SKY_ANGLE, F_SKY_COLOR, F_BACK_URL, F_BOTTOM_URL, F_FRONT_URL, F_LEFT_URL, F_RIGHT_URL, F_TOP_URL, F_AVATAR_SIZE, F_HEADLIGHT, F_SPEED, F_TYPE, F_VISIBILITY_LIMIT, F_FIELD_OF_VIEW, F_JUMP, F_ORIENTATION, F_POSITION, F_DESCRIPTION, F_CHILDREN, F_ADD_CHILDREN, F_REMOVE_CHILDREN, F_BBOX_CENTER, F_BBOX_SIZE, F_CENTER, F_ROTATION, F_SCALE, F_SCALE_ORIENTATION, F_TRANSLATION, F_APPEARANCE, F_GEOMETRY, F_MATERIAL, F_TEXTURE, F_TEXTURE_TRANSFORM, F_AMBIENT_INTENSITY, F_DIFFUSE_COLOR, F_EMISSIVE_COLOR, F_SHININESS, F_SPECULAR_COLOR, F_TRANSPARANCY, F_REPEAT_S, F_REPEAT_T, F_SIZE, F_BOTTOM, F_BOTTOM_RADIUS, F_HEIGHT, F_SIDE, F_RADIUS, F_TOP, F_STRING, F_FONT_STYLE, F_LENGTH, F_MAX_EXTENT, F_FAMILY, F_HORIZONTAL, F_JUSTIFY, F_LANGUAGE, F_LEFT_TO_RIGHT, F_SPACING, F_STYLE, F_TOP_TO_BOTTOM, F_COLOR, F_COORD, F_COLOR_INDEX, F_COLOR_PER_VERTEX, F_COORD_INDEX, F_CCW, F_CONVEX, F_SOLID, F_CREASE_ANGLE, F_NORMAL, F_NORMAL_INDEX, F_NORMAL_PER_VERTEX, F_TEX_COORD_INDEX, F_TEX_COORD, F_POINT, F_VECTOR, F_BBOXCENTER, F_BBOXSIZE, F_AUTO_OFFSET, F_DISK_ANGLE, F_ENABLED, F_MAX_ANGLE, F_MIN_ANGLE, F_OFFSET, F_IMAGE, F_X_DIMENSION, F_Z_DIMENSION, F_X_SPACING, F_Z_SPACING, F_LOOP, F_START_TIME, F_STOP_TIME, F_CROSS_SECTION, F_SPINE, F_BEGIN_CAP, F_END_CAP, F_CHOICE, F_WHICH_CHOICE, F_RANGE, F_LEVEL, F_COLLIDE, F_PROXY, F_PARAMETER, F_VISIBILITY_RANGE, F_FOG_TYPE, F_AXIS_OF_ROTATION, F_TITLE, F_INFO, F_LOCATION, F_ON, F_INTENSITY, F_ATTENUATION, F_DIRECTION, F_BEAM_WIDTH, F_CUT_OFF_RANGE, // event type E_FIELD, E_EXPOSED_FIELD, E_EVENTIN, E_EVENTOUT, // def & route D_DEF, D_USE, D_ROUTE, // unsupported keywords U_SCRIPT, U_EXTERNPROTO }; } namespace cnoid { class VrmlParserImpl { public: VrmlParserImpl(VrmlParser* self); VrmlParser* self; EasyScannerPtr scanner; VrmlProtoInstancePtr currentProtoInstance; bool protoInstanceActualNodeExtractionMode; typedef map ProtoToEntityScannerMap; ProtoToEntityScannerMap protoToEntityScannerMap; typedef map TDefNodeMap; typedef pair TDefNodePair; typedef map TProtoMap; typedef pair TProtoPair; TProtoMap protoMap; TDefNodeMap defNodeMap; void load(const string& filename); VrmlNodePtr readSpecificNode(VrmlNodeCategory nodeCategory, int symbol, const std::string& symbolString); VrmlNodePtr readInlineNode(VrmlNodeCategory nodeCategory); VrmlNodePtr newInlineSource(std::string& io_filename); VrmlProtoPtr defineProto(); VrmlNodePtr readNode(VrmlNodeCategory nodeCategory); VrmlProtoInstancePtr readProtoInstanceNode(const std::string& proto_name, VrmlNodeCategory nodeCategory); VrmlNodePtr evalProtoInstance(VrmlProtoInstancePtr proto, VrmlNodeCategory nodeCategory); VrmlUnsupportedNodePtr skipUnsupportedNode(const std::string& nodeTypeName); VrmlUnsupportedNodePtr skipScriptNode(); VrmlUnsupportedNodePtr skipExternProto(); VrmlViewpointPtr readViewpointNode(); VrmlNavigationInfoPtr readNavigationInfoNode(); VrmlBackgroundPtr readBackgroundNode(); VrmlGroupPtr readGroupNode(); VrmlTransformPtr readTransformNode(); VrmlShapePtr readShapeNode(); VrmlCylinderSensorPtr readCylinderSensorNode(); VrmlBoxPtr readBoxNode(); VrmlConePtr readConeNode(); VrmlCylinderPtr readCylinderNode(); VrmlPointSetPtr readPointSetNode(); VrmlPixelTexturePtr readPixelTextureNode(); VrmlMovieTexturePtr readMovieTextureNode(); VrmlElevationGridPtr readElevationGridNode(); VrmlExtrusionPtr readExtrusionNode(); VrmlSwitchPtr readSwitchNode(); VrmlLODPtr readLODNode(); VrmlCollisionPtr readCollisionNode(); VrmlAnchorPtr readAnchorNode(); VrmlFogPtr readFogNode(); VrmlBillboardPtr readBillboardNode(); VrmlWorldInfoPtr readWorldInfoNode(); VrmlPointLightPtr readPointLightNode(); VrmlDirectionalLightPtr readDirectionalLightNode(); VrmlSpotLightPtr readSpotLightNode(); VrmlSpherePtr readSphereNode(); VrmlTextPtr readTextNode(); VrmlFontStylePtr readFontStyleNode(); VrmlIndexedLineSetPtr readIndexedLineSetNode(); VrmlIndexedFaceSetPtr readIndexedFaceSetNode(); void checkIndexedFaceSet(VrmlIndexedFaceSetPtr node); VrmlCoordinatePtr readCoordNode(); VrmlTextureCoordinatePtr readTextureCoordinateNode(); VrmlColorPtr readColorNode(); VrmlAppearancePtr readAppearanceNode(); VrmlMaterialPtr readMaterialNode(); VrmlImageTexturePtr readImageTextureNode(); VrmlTextureTransformPtr readTextureTransformNode(); VrmlNormalPtr readNormalNode(); VrmlVariantField& readProtoField(VrmlFieldTypeId fieldTypeId); void readSFInt32(SFInt32& out_value); void readMFInt32(MFInt32& out_value); void readSFFloat(SFFloat& out_value); void readMFFloat(MFFloat& out_value); void readSFColor(SFColor& out_value); void readMFColor(MFColor& out_value); void readSFString(SFString& out_value); void readMFString(MFString& out_value); void readSFVec2f(SFVec2f& out_value); void readMFVec2f(MFVec2f& out_value); void readSFVec3f(SFVec3f& out_value); void readMFVec3f(MFVec3f& out_value); void readSFRotation(SFRotation& out_value); void readMFRotation(MFRotation& out_value); void readSFBool(SFBool& out_value); void readSFTime(SFTime& out_value); void readMFTime(MFTime& out_value); void readSFNode(SFNode& out_node, VrmlNodeCategory nodeCategory); SFNode readSFNode(VrmlNodeCategory nodeCategory); void readMFNode(MFNode& out_nodes, VrmlNodeCategory nodeCategory); void readSFImage( SFImage& out_image ); private: VrmlParserImpl(const VrmlParserImpl& self, const list< string >& ref); const list< string >* getAncestorPathsList() const {return &ancestorPathsList;} void setSymbols(); void init(); list ancestorPathsList; }; } VrmlParser::VrmlParser() { init(); } VrmlParser::VrmlParser(const string& filename) { init(); load(filename); } void VrmlParser::init() { impl = new VrmlParserImpl(this); } VrmlParserImpl::VrmlParserImpl(VrmlParser* self) : self(self) { init(); } VrmlParserImpl::VrmlParserImpl(const VrmlParserImpl& refThis, const list< string >& refSet) : self(refThis.self), ancestorPathsList(refSet) { init(); } VrmlParser::~VrmlParser() { delete impl; } void VrmlParser::setProtoInstanceActualNodeExtractionMode(bool isOn) { impl->protoInstanceActualNodeExtractionMode = isOn; } /** This function throws EasyScanner::Exception when an error occurs. */ void VrmlParser::load(const string& filename) { impl->load(filename); } void VrmlParserImpl::load(const string& filename) { filesystem::path path(filename); path.normalize(); string pathString(path.file_string()); ancestorPathsList.push_back(pathString); scanner->loadFile(pathString); // header check scanner->setCommentChar(0); bool ok = scanner->readString("#VRML V2.0"); if(ok){ scanner->skipLine(); } scanner->setCommentChar('#'); scanner->setQuoteChar('"'); scanner->setWhiteSpaceChar(','); scanner->setLineOriented(false); } VrmlNodePtr VrmlParser::readNode() { return impl->readNode(TOP_NODE); } VrmlNodePtr VrmlParserImpl::readNode(VrmlNodeCategory nodeCategory) { VrmlNodePtr node; if(scanner->isEOF()){ if(currentProtoInstance){ scanner->throwException("Illegal proto instance node"); } } if(!scanner->readWord()){ return 0; } string def_name; string nodeTypeName(scanner->stringValue); int symbol = scanner->getSymbolID(scanner->stringValue); if(symbol){ if(symbol==N_INLINE) return readInlineNode(nodeCategory); if(symbol==N_PROTO){ if(nodeCategory == TOP_NODE){ return defineProto(); } else { scanner->throwException("PROTO node cannot be defined here"); } } if(symbol==D_USE){ scanner->readString(); const string& label = scanner->stringValue; TDefNodeMap::iterator p = defNodeMap.find(label); if(p != defNodeMap.end()){ return p->second; } else { scanner->throwException (string("A node \"") + label + "\" specified by the USE directive does not exist"); } } // ROUTE has 3 parameters to skip // return as VrmlUnsupportedNode if(symbol == D_ROUTE){ scanner->readString(); // eventOut or exposedField if(!scanner->readString("TO")) // "TO" scanner->throwException("Illegal ROUTE (without TO)"); scanner->readString(); // eventIn or exposedField // recursive call to continue reading without node construction return readNode( nodeCategory ); } // unsupported keywords if(symbol == U_SCRIPT){ cerr << "Script is not supported. " << endl; skipScriptNode(); return readNode( nodeCategory ); } if(symbol == U_EXTERNPROTO){ cerr << "ExternProto is not supported." << endl; skipExternProto(); return readNode( nodeCategory ); } if(symbol == D_DEF){ def_name = scanner->readStringEx("Illegal DEF name"); scanner->readWord(); symbol = scanner->getSymbolID(scanner->stringValue); nodeTypeName = scanner->stringValue; } } if(!scanner->readChar('{')){ scanner->throwException (string("The entity of a ") + nodeTypeName + " node does not correctly begin with '{'"); } if(symbol){ node = readSpecificNode(nodeCategory, symbol, nodeTypeName); } else { node = readProtoInstanceNode(scanner->stringValue, nodeCategory); } if(!scanner->readChar('}')){ scanner->throwException (string("A ") + nodeTypeName + " node is not correctly closed with '}'"); } if(def_name.size() > 0) { defNodeMap.insert(TDefNodePair(def_name, node)); node->defName = def_name; } return node; } VrmlNodePtr VrmlParserImpl::readSpecificNode(VrmlNodeCategory nodeCategory, int symbol, const string& nodeTypeName) { VrmlNodePtr node; switch(symbol){ case N_BACKGROUND: node = readBackgroundNode(); break; case N_NAVIGATION_INFO: node = readNavigationInfoNode(); break; case N_VIEWPOINT: node = readViewpointNode(); break; case N_GROUP: node = readGroupNode(); break; case N_TRANSFORM: node = readTransformNode(); break; case N_SHAPE: node = readShapeNode(); break; case N_CYLINDER_SENSOR: node = readCylinderSensorNode(); break; case N_POINTSET: node = readPointSetNode(); break; case N_PIXEL_TEXTURE: node = readPixelTextureNode(); break; case N_MOVIE_TEXTURE: node = readMovieTextureNode(); break; case N_ELEVATION_GRID: node = readElevationGridNode(); break; case N_EXTRUSION: node = readExtrusionNode(); break; case N_SWITCH: node = readSwitchNode(); break; case N_LOD: node = readLODNode(); break; case N_COLLISION: node = readCollisionNode(); break; case N_ANCHOR: node = readAnchorNode(); break; case N_FOG: node = readFogNode(); break; case N_BILLBOARD: node = readBillboardNode(); break; case N_WORLD_INFO: node = readWorldInfoNode(); break; case N_POINT_LIGHT: node = readPointLightNode(); break; case N_DIRECTIONAL_LIGHT: node = readDirectionalLightNode(); break; case N_SPOT_LIGHT: node = readSpotLightNode(); break; case N_MATERIAL: node = readMaterialNode(); break; case N_APPEARANCE: node = readAppearanceNode(); break; case N_IMAGE_TEXTURE: node = readImageTextureNode(); break; case N_TEXTURE_TRANSFORM: node = readTextureTransformNode(); break; case N_BOX: node = readBoxNode(); break; case N_CONE: node = readConeNode(); break; case N_CYLINDER: node = readCylinderNode(); break; case N_SPHERE: node = readSphereNode(); break; case N_TEXT: node = readTextNode(); break; case N_INDEXED_FACE_SET: node = readIndexedFaceSetNode(); break; case N_INDEXED_LINE_SET: node = readIndexedLineSetNode(); break; case N_COORDINATE: node = readCoordNode(); break; case N_TEXTURE_COORDINATE: node = readTextureCoordinateNode(); break; case N_COLOR: node = readColorNode(); break; case N_NORMAL: node = readNormalNode(); break; case N_FONT_STYLE: node = readFontStyleNode(); break; // unsupported nodes case N_AUDIO_CLIP: node = skipUnsupportedNode("AudioClip"); break; case N_SOUND: node = skipUnsupportedNode("Sound"); break; case N_COLOR_INTERPOLATOR: node = skipUnsupportedNode("ColorInterpolator"); break; case N_COORDINATE_INTERPOLATOR: node = skipUnsupportedNode("CoordinateInterpolator"); break; case N_ORIENTATION_INTERPOLATOR: node = skipUnsupportedNode("OrientationInterpolator"); break; case N_NORMAL_INTERPOLATOR: node = skipUnsupportedNode("NormalInterpolator"); break; case N_POSITION_INTERPOLATOR: node = skipUnsupportedNode("PositionInterpolator"); break; case N_SCALAR_INTERPOLATOR: node = skipUnsupportedNode("ScalarInterpolator"); break; case N_PLANE_SENSOR: node = skipUnsupportedNode("PlaneSensor"); break; case N_PROXIMITY_SENSOR: node = skipUnsupportedNode("ProximitySensor"); break; case N_SPHERE_SENSOR: node = skipUnsupportedNode("SphereSensor"); break; case N_TIME_SENSOR: node = skipUnsupportedNode("TimeSensor"); break; case N_TOUCH_SENSOR: node = skipUnsupportedNode("TouchSensor"); break; case N_VISIBILITY_SENSOR: node = skipUnsupportedNode("VisibilitySensor"); break; default: scanner->throwException (string("Node type \"") + nodeTypeName + "\" is not supported"); } if(!node->isCategoryOf(nodeCategory)){ scanner->throwException (string("A ") + nodeTypeName + " node is put in a illegal place"); } return node; } VrmlUnsupportedNodePtr VrmlParserImpl::skipUnsupportedNode(const std::string& nodeTypeName) { while(true){ if(!scanner->readQuotedString()){ if(scanner->peekChar() == '}'){ break; } if(!scanner->readChar()){ scanner->throwException( "Node is not closed." ); } } } return new VrmlUnsupportedNode( nodeTypeName ); } VrmlUnsupportedNodePtr VrmlParserImpl::skipScriptNode() { // '}' appears twice in "Script" node for(int i=0; i<1; i++){ while(true){ if(!scanner->readQuotedString()){ if(scanner->peekChar() == '}'){ scanner->readChar(); break; } if(!scanner->readChar()){ scanner->throwException( "Script is not closed." ); } } } } // return new VrmlUnsupportedNode( "Script" ); return NULL; } VrmlUnsupportedNodePtr VrmlParserImpl::skipExternProto() { // read untill ']' appears while(true){ if(!scanner->readQuotedString()){ if( scanner->peekChar() == ']' ){ // read found ']' and break this loop scanner->readChar(); break; } if(!scanner->readChar()){ scanner->throwException( "EXTERNPROTO is not closed." ); } } } // read URL after ']' SFString url; readSFString( url ); // return new VrmlUnsupportedNode( "EXTERNPROTO" ); return NULL; } VrmlNodePtr VrmlParserImpl::readInlineNode(VrmlNodeCategory nodeCategory) { scanner->readChar('{'); if(scanner->readSymbol() && scanner->symbolValue == F_URL){ MFString inlineUrls; readMFString(inlineUrls); scanner->readCharEx('}', "syntax error 2"); VrmlInlinePtr inlineNode = new VrmlInline(); for(size_t i=0; i < inlineUrls.size(); ++i){ string url(fromUtf8(inlineUrls[i])); inlineNode->children.push_back(newInlineSource(url)); inlineNode->urls.push_back(url); } return inlineNode; } return 0; } VrmlNodePtr VrmlParserImpl::newInlineSource(string& io_filename) { filesystem::path path; string chkFile(""); if(isFileProtocol(io_filename)){ path = filesystem::path(deleteURLScheme(io_filename)); path.normalize(); // Relative path check & translate to absolute path if(!exists(path)){ filesystem::path parentPath(scanner->filename); #if BOOST_VERSION < 103600 path = parentPath.branch_path() / path; #else path = parentPath.parent_path() / path; #endif path.normalize(); } chkFile = complete(path).string(); } else { // Not file protocol implements chkFile = io_filename; } for(list::const_iterator p = ancestorPathsList.begin(); p != ancestorPathsList.end(); ++p){ if(*p == chkFile){ scanner->throwException("Infinity loop ! " + chkFile + " is included ancestor list"); } } VrmlParserImpl inlineParser(*this, ancestorPathsList); inlineParser.load(chkFile); io_filename = chkFile; VrmlGroupPtr group = new VrmlGroup(); while(VrmlNodePtr node = inlineParser.readNode(TOP_NODE)){ if(node->isCategoryOf(CHILD_NODE)){ group->children.push_back(node); } } if(group->children.size() == 1){ return group->children.front(); } else { return group; } } VrmlProtoPtr VrmlParserImpl::defineProto() { string proto_name = scanner->readWordEx("illegal PROTO name"); scanner->readCharEx('[', "syntax error 3"); VrmlProtoPtr proto = new VrmlProto(proto_name); while(!scanner->readChar(']')){ int event_type = scanner->readSymbolEx("illegal field event type"); int field_symbol = scanner->readSymbolEx("illegal field type"); string field_name = scanner->readWordEx("syntax error 4"); // insert a new empty field and get it, contents of which will be set below VrmlVariantField& field = proto->fields.insert(TProtoFieldPair(field_name, VrmlVariantField())).first->second; switch(event_type){ case E_FIELD: case E_EXPOSED_FIELD: switch(field_symbol){ case T_SFINT32: field = SFInt32(); readSFInt32(get(field)); break; case T_SFFLOAT: field = SFFloat(); readSFFloat(get(field)); break; case T_MFINT32: field = MFInt32(); readMFInt32(get(field)); break; case T_MFFLOAT: field = MFFloat(); readMFFloat(get(field)); break; case T_SFVEC3F: field = SFVec3f(); readSFVec3f(get(field)); break; case T_MFVEC3F: field = MFVec3f(); readMFVec3f(get(field)); break; case T_SFCOLOR: field = SFColor(); readSFColor(get(field)); break; case T_MFCOLOR: field = MFColor(); readMFColor(get(field)); break; case T_SFSTRING: field = SFString(); readSFString(get(field)); break; case T_MFSTRING: field = MFString(); readMFString(get(field)); break; case T_SFROTATION: field = SFRotation(); readSFRotation(get(field)); break; case T_MFROTATION: field = MFRotation(); readMFRotation(get(field)); break; case T_SFBOOL: field = SFBool(); readSFBool(get(field)); break; case T_SFNODE: field = SFNode(); readSFNode(get(field), ANY_NODE); break; case T_MFNODE: field = MFNode(); readMFNode(get(field), ANY_NODE); break; case T_SFIMAGE: field = SFImage(); readSFImage(get(field)); break; default: scanner->throwException("illegal field type"); } break; case E_EVENTIN: case E_EVENTOUT: switch(field_symbol){ case T_SFINT32: field = SFInt32(); break; case T_MFINT32: field = MFInt32(); break; case T_SFFLOAT: field = SFFloat(); break; case T_MFFLOAT: field = MFFloat(); break; case T_SFVEC3F: field = SFVec3f(); break; case T_MFVEC3F: field = MFVec3f(); break; case T_SFCOLOR: field = SFColor(); break; case T_MFCOLOR: field = MFColor(); break; case T_SFSTRING: field = SFString(); break; case T_MFSTRING: field = MFString(); break; case T_SFROTATION: field = SFRotation(); break; case T_MFROTATION: field = MFRotation(); break; case T_SFBOOL: field = SFBool(); break; case T_SFNODE: field = SFNode(); break; case T_MFNODE: field = MFNode(); break; case T_SFIMAGE: field = SFImage(); break; } break; default: scanner->throwException("illegal field event type"); } } scanner->readCharEx('{', "A PROTO definition has no entity"); char* begin = scanner->text; int brace_level = 1; while(true){ int token = scanner->readToken(); if(token == EasyScanner::T_NONE){ scanner->throwException("syntax error 5"); } else if(token == EasyScanner::T_SIGLUM){ if(scanner->charValue == '{') { brace_level++; } else if(scanner->charValue == '}') { brace_level--; if(brace_level==0) break; } } } EasyScannerPtr entityScanner(new EasyScanner(*scanner, false)); entityScanner->setText(begin, scanner->text - begin - 1); entityScanner->setLineNumberOffset(scanner->lineNumber); protoToEntityScannerMap[proto.get()] = entityScanner; protoMap.insert(TProtoPair(proto_name, proto)); return proto; } VrmlProtoInstancePtr VrmlParserImpl::readProtoInstanceNode(const string& proto_name, VrmlNodeCategory nodeCategory) { TProtoMap::iterator p = protoMap.find(proto_name); if(p == protoMap.end()){ scanner->throwException("undefined node"); } VrmlProtoPtr proto = p->second; VrmlProtoInstancePtr protoInstance(new VrmlProtoInstance(proto)); while(scanner->readWord()){ TProtoFieldMap::iterator p = protoInstance->fields.find(scanner->stringValue); if(p == protoInstance->fields.end()) scanner->throwException("undefined field"); VrmlVariantField& field = p->second; switch(field.which()){ case SFINT32: readSFInt32(get(field)); break; case MFINT32: readMFInt32(get(field)); break; case SFFLOAT: readSFFloat(get(field)); break; case MFFLOAT: readMFFloat(get(field)); break; case SFVEC2F: readSFVec2f(get(field)); break; case MFVEC2F: readMFVec2f(get(field)); break; case SFVEC3F: readSFVec3f(get(field)); break; case MFVEC3F: readMFVec3f(get(field)); break; case SFCOLOR: readSFColor(get(field)); break; case MFCOLOR: readMFColor(get(field)); break; case SFSTRING: readSFString(get(field)); break; case MFSTRING: readMFString(get(field)); break; case SFROTATION: readSFRotation(get(field)); break; case MFROTATION: readMFRotation(get(field)); break; case SFBOOL: readSFBool(get(field)); break; case SFNODE: readSFNode(get(field), ANY_NODE); break; case MFNODE: readMFNode(get(field), ANY_NODE); break; case SFIMAGE: readSFImage(get(field)); break; default: break; } } if(protoInstanceActualNodeExtractionMode){ protoInstance->actualNode = evalProtoInstance(protoInstance, nodeCategory); } return protoInstance; } VrmlNodePtr VrmlParserImpl::evalProtoInstance(VrmlProtoInstancePtr protoInstance, VrmlNodeCategory nodeCategory) { EasyScannerPtr orgScanner = scanner; ProtoToEntityScannerMap::iterator p; p = protoToEntityScannerMap.find(protoInstance->proto.get()); if(p == protoToEntityScannerMap.end()){ scanner->throwException("Undefined proto node instance"); } scanner = p->second; scanner->moveToHead(); VrmlProtoInstancePtr orgProtoInstance = currentProtoInstance; currentProtoInstance = protoInstance; VrmlNodePtr node = readNode(nodeCategory); node->defName = protoInstance->defName; scanner = orgScanner; currentProtoInstance = orgProtoInstance; return node; } VrmlViewpointPtr VrmlParserImpl::readViewpointNode() { VrmlViewpointPtr node(new VrmlViewpoint); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_FIELD_OF_VIEW: readSFFloat(node->fieldOfView); break; case F_JUMP: readSFBool(node->jump); break; case F_ORIENTATION: readSFRotation(node->orientation); break; case F_POSITION: readSFVec3f(node->position); break; case F_DESCRIPTION: readSFString(node->description); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlNavigationInfoPtr VrmlParserImpl::readNavigationInfoNode() { VrmlNavigationInfoPtr node(new VrmlNavigationInfo); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_AVATAR_SIZE: readMFFloat(node->avatarSize); break; case F_HEADLIGHT: readSFBool(node->headlight); break; case F_SPEED: readSFFloat(node->speed); break; case F_TYPE: readMFString(node->type); break; case F_VISIBILITY_LIMIT: readSFFloat(node->visibilityLimit); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlBackgroundPtr VrmlParserImpl::readBackgroundNode() { VrmlBackgroundPtr node(new VrmlBackground); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_GROUND_ANGLE: readMFFloat(node->groundAngle); break; case F_GROUND_COLOR: readMFColor(node->groundColor); break; case F_SKY_ANGLE: readMFFloat(node->skyAngle); break; case F_SKY_COLOR: readMFColor(node->skyColor); break; case F_BACK_URL: readMFString(node->backUrl); break; case F_BOTTOM_URL: readMFString(node->bottomUrl); break; case F_FRONT_URL: readMFString(node->frontUrl); break; case F_LEFT_URL: readMFString(node->leftUrl); break; case F_RIGHT_URL: readMFString(node->rightUrl); break; case F_TOP_URL: readMFString(node->topUrl); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlGroupPtr VrmlParserImpl::readGroupNode() { VrmlGroupPtr node(new VrmlGroup); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_BBOX_CENTER: readSFVec3f(node->bboxCenter); break; case F_BBOX_SIZE: readSFVec3f(node->bboxSize); break; case F_CHILDREN: readMFNode(node->children, CHILD_NODE); break; case F_ADD_CHILDREN: case F_REMOVE_CHILDREN: { MFNode dummy; readMFNode(dummy, CHILD_NODE); } break; default: scanner->throwException("Undefined field"); } } return node; } VrmlTransformPtr VrmlParserImpl::readTransformNode() { VrmlTransformPtr node(new VrmlTransform); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_CENTER: readSFVec3f(node->center); break; case F_ROTATION: readSFRotation(node->rotation); break; case F_SCALE: readSFVec3f(node->scale); break; case F_SCALE_ORIENTATION: readSFRotation(node->scaleOrientation); break; case F_TRANSLATION: readSFVec3f(node->translation); break; case F_BBOX_CENTER: readSFVec3f(node->bboxCenter); break; case F_BBOX_SIZE: readSFVec3f(node->bboxSize); break; case F_CHILDREN: readMFNode(node->children, CHILD_NODE); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlShapePtr VrmlParserImpl::readShapeNode() { VrmlShapePtr node(new VrmlShape); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_APPEARANCE: node->appearance = dynamic_pointer_cast(readSFNode(APPEARANCE_NODE)); break; case F_GEOMETRY: node->geometry = readSFNode(GEOMETRY_NODE); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlCylinderSensorPtr VrmlParserImpl::readCylinderSensorNode() { VrmlCylinderSensorPtr node(new VrmlCylinderSensor); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_AUTO_OFFSET: readSFBool(node->autoOffset); break; case F_DISK_ANGLE: readSFFloat(node->diskAngle); break; case F_ENABLED: readSFBool(node->enabled); break; case F_MAX_ANGLE: readSFFloat(node->maxAngle); break; case F_MIN_ANGLE: readSFFloat(node->minAngle); break; case F_OFFSET: readSFFloat(node->offset); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlPointSetPtr VrmlParserImpl::readPointSetNode() { VrmlPointSetPtr node(new VrmlPointSet); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_COORD: node->coord = dynamic_pointer_cast(readSFNode(COORDINATE_NODE)); break; case F_COLOR: node->color = dynamic_pointer_cast(readSFNode(COLOR_NODE)); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlPixelTexturePtr VrmlParserImpl::readPixelTextureNode() { VrmlPixelTexturePtr node(new VrmlPixelTexture); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_IMAGE: readSFImage(node->image); break; case F_REPEAT_S: readSFBool(node->repeatS); break; case F_REPEAT_T: readSFBool(node->repeatT); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlMovieTexturePtr VrmlParserImpl::readMovieTextureNode() { VrmlMovieTexturePtr node(new VrmlMovieTexture); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_URL: readMFString(node->url); break; case F_LOOP: readSFBool(node->loop); break; case F_SPEED: readSFFloat(node->speed); break; case F_START_TIME: readSFTime(node->startTime); break; case F_STOP_TIME: readSFTime(node->stopTime); break; case F_REPEAT_S: readSFBool(node->repeatS); break; case F_REPEAT_T: readSFBool(node->repeatT); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlElevationGridPtr VrmlParserImpl::readElevationGridNode() { VrmlElevationGridPtr node(new VrmlElevationGrid); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_X_DIMENSION: readSFInt32(node->xDimension); break; case F_Z_DIMENSION: readSFInt32(node->zDimension); break; case F_X_SPACING: readSFFloat(node->xSpacing); break; case F_Z_SPACING: readSFFloat(node->zSpacing); break; case F_HEIGHT: readMFFloat(node->height); break; case F_CCW: readSFBool(node->ccw); break; case F_COLOR_PER_VERTEX: readSFBool(node->colorPerVertex); break; case F_CREASE_ANGLE: readSFFloat(node->creaseAngle); break; case F_NORMAL_PER_VERTEX: readSFBool(node->normalPerVertex); break; case F_SOLID: readSFBool(node->solid); break; case F_COLOR: node->color = dynamic_pointer_cast(readSFNode(COLOR_NODE)); break; case F_NORMAL: node->normal = dynamic_pointer_cast(readSFNode(NORMAL_NODE)); break; case F_TEX_COORD: node->texCoord = dynamic_pointer_cast(readSFNode(TEXTURE_COORDINATE_NODE)); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlExtrusionPtr VrmlParserImpl::readExtrusionNode() { VrmlExtrusionPtr node(new VrmlExtrusion); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_CROSS_SECTION: readMFVec2f(node->crossSection); break; case F_SPINE: readMFVec3f(node->spine); break; case F_SCALE: readMFVec2f(node->scale); break; case F_ORIENTATION: readMFRotation(node->orientation); break; case F_BEGIN_CAP: readSFBool(node->beginCap); break; case F_END_CAP: readSFBool( node->endCap ); break; case F_SOLID: readSFBool( node->solid ); break; case F_CCW: readSFBool( node->ccw ); break; case F_CONVEX: readSFBool( node->convex ); break; case F_CREASE_ANGLE: readSFFloat( node->creaseAngle ); break; default: scanner->throwException( "Undefined field" ); } } return node; } VrmlSwitchPtr VrmlParserImpl::readSwitchNode() { VrmlSwitchPtr node( new VrmlSwitch ); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_CHOICE: readMFNode(node->choice, CHILD_NODE); break; case F_WHICH_CHOICE: readSFInt32(node->whichChoice); break; default: scanner->throwException( "Undefined field" ); } } return node; } VrmlLODPtr VrmlParserImpl::readLODNode() { VrmlLODPtr node( new VrmlLOD ); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_RANGE: readMFFloat(node->range); break; case F_CENTER: readSFVec3f(node->center); break; case F_LEVEL: readMFNode(node->level, ANY_NODE); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlCollisionPtr VrmlParserImpl::readCollisionNode() { VrmlCollisionPtr node(new VrmlCollision); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_COLLIDE: readSFBool(node->collide); break; case F_CHILDREN: readMFNode(node->children, CHILD_NODE); break; case F_PROXY: readSFNode(node->proxy, SHAPE_NODE); break; case F_BBOX_CENTER: readSFVec3f(node->bboxCenter); break; case F_BBOX_SIZE: readSFVec3f(node->bboxSize); break; default: scanner->throwException( "Undefined field" ); } } return node; } VrmlAnchorPtr VrmlParserImpl::readAnchorNode() { VrmlAnchorPtr node(new VrmlAnchor); while(scanner->readSymbol()){ switch( scanner->symbolValue){ case F_CHILDREN: readMFNode(node->children, CHILD_NODE); break; case F_DESCRIPTION: readSFString(node->description); break; case F_PARAMETER: readMFString(node->parameter); break; case F_URL: readMFString(node->url); break; case F_BBOX_CENTER: readSFVec3f(node->bboxCenter); break; case F_BBOX_SIZE: readSFVec3f(node->bboxSize); break; default: scanner->throwException( "Undefined field" ); } } return node; } VrmlFogPtr VrmlParserImpl::readFogNode() { VrmlFogPtr node(new VrmlFog); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_COLOR: readSFColor(node->color); break; case F_VISIBILITY_RANGE: readSFFloat(node->visibilityRange); break; case F_FOG_TYPE: readSFString(node->fogType); break; default: scanner->throwException( "Undefined field" ); } } return node; } VrmlBillboardPtr VrmlParserImpl::readBillboardNode() { VrmlBillboardPtr node( new VrmlBillboard ); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_AXIS_OF_ROTATION: readSFVec3f(node->axisOfRotation); break; case F_CHILDREN: readMFNode(node->children, CHILD_NODE); break; case F_BBOX_CENTER: readSFVec3f(node->bboxCenter); break; case F_BBOX_SIZE: readSFVec3f(node->bboxSize); break; default: scanner->throwException( "Undefined field" ); } } return node; } VrmlWorldInfoPtr VrmlParserImpl::readWorldInfoNode() { VrmlWorldInfoPtr node(new VrmlWorldInfo); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_TITLE: readSFString(node->title); break; case F_INFO: readMFString(node->info); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlPointLightPtr VrmlParserImpl::readPointLightNode() { VrmlPointLightPtr node( new VrmlPointLight ); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_LOCATION: readSFVec3f(node->location); break; case F_ON: readSFBool(node->on); break; case F_INTENSITY: readSFFloat(node->intensity); break; case F_COLOR: readSFColor(node->color); break; case F_RADIUS: readSFFloat(node->radius); break; case F_AMBIENT_INTENSITY: readSFFloat(node->ambientIntensity); break; case F_ATTENUATION: readSFVec3f(node->attenuation); break; default: scanner->throwException( "Undefined field" ); } } return node; } VrmlDirectionalLightPtr VrmlParserImpl::readDirectionalLightNode() { VrmlDirectionalLightPtr node(new VrmlDirectionalLight); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_DIRECTION: readSFVec3f(node->direction); break; case F_ON: readSFBool(node->on); break; case F_INTENSITY: readSFFloat(node->intensity); break; case F_COLOR: readSFColor(node->color); break; case F_AMBIENT_INTENSITY: readSFFloat(node->ambientIntensity); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlSpotLightPtr VrmlParserImpl::readSpotLightNode() { VrmlSpotLightPtr node(new VrmlSpotLight); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_LOCATION: readSFVec3f(node->location); break; case F_DIRECTION: readSFVec3f(node->direction); break; case F_ON: readSFBool(node->on); break; case F_COLOR: readSFColor(node->color); break; case F_INTENSITY: readSFFloat(node->intensity); break; case F_RADIUS: readSFFloat(node->radius); break; case F_AMBIENT_INTENSITY: readSFFloat(node->ambientIntensity); break; case F_ATTENUATION: readSFVec3f(node->attenuation); break; case F_BEAM_WIDTH: readSFFloat(node->beamWidth); break; case F_CUT_OFF_RANGE: readSFFloat(node->cutOffAngle); break; default: scanner->throwException( "Undefined field" ); } } return node; } VrmlBoxPtr VrmlParserImpl::readBoxNode() { VrmlBoxPtr node(new VrmlBox); if(scanner->readSymbol()){ if(scanner->symbolValue != F_SIZE) scanner->throwException("Undefined field"); readSFVec3f(node->size); } return node; } VrmlConePtr VrmlParserImpl::readConeNode() { VrmlConePtr node(new VrmlCone); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_BOTTOM: readSFBool(node->bottom); break; case F_BOTTOM_RADIUS: readSFFloat(node->bottomRadius); break; case F_HEIGHT: readSFFloat(node->height); break; case F_SIDE: readSFBool(node->side); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlCylinderPtr VrmlParserImpl::readCylinderNode() { VrmlCylinderPtr node(new VrmlCylinder); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_BOTTOM: readSFBool(node->bottom); break; case F_HEIGHT: readSFFloat(node->height); break; case F_RADIUS: readSFFloat(node->radius); break; case F_SIDE: readSFBool(node->side); break; case F_TOP: readSFBool(node->top); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlSpherePtr VrmlParserImpl::readSphereNode() { VrmlSpherePtr node(new VrmlSphere); if(scanner->readSymbol()){ if(scanner->symbolValue != F_RADIUS) scanner->throwException("Undefined field"); readSFFloat(node->radius); } return node; } VrmlTextPtr VrmlParserImpl::readTextNode() { VrmlTextPtr node(new VrmlText); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_STRING: readMFString(node->fstring); break; case F_LENGTH: readMFFloat(node->length); break; case F_MAX_EXTENT: readSFFloat(node->maxExtent); break; case F_FONT_STYLE: node->fontStyle = dynamic_pointer_cast(readSFNode(FONT_STYLE_NODE)); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlFontStylePtr VrmlParserImpl::readFontStyleNode() { VrmlFontStylePtr node(new VrmlFontStyle); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_FAMILY: readMFString(node->family); break; case F_HORIZONTAL: readSFBool(node->horizontal); break; case F_JUSTIFY: readMFString(node->justify); break; case F_LANGUAGE: readSFString(node->language); break; case F_LEFT_TO_RIGHT: readSFBool(node->leftToRight); break; case F_SIZE: readSFFloat(node->size); break; case F_SPACING: readSFFloat(node->spacing); break; case F_STYLE: readSFString(node->style); break; case F_TOP_TO_BOTTOM: readSFBool(node->topToBottom); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlIndexedLineSetPtr VrmlParserImpl::readIndexedLineSetNode() { VrmlIndexedLineSetPtr node(new VrmlIndexedLineSet); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_COLOR: node->color = dynamic_pointer_cast(readSFNode(COLOR_NODE)); break; case F_COORD: node->coord = dynamic_pointer_cast(readSFNode(COORDINATE_NODE)); break; case F_COLOR_INDEX: readMFInt32(node->colorIndex); break; case F_COLOR_PER_VERTEX: readSFBool(node->colorPerVertex); break; case F_COORD_INDEX: readMFInt32(node->coordIndex); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlIndexedFaceSetPtr VrmlParserImpl::readIndexedFaceSetNode() { VrmlIndexedFaceSetPtr node(new VrmlIndexedFaceSet); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_COLOR: node->color = dynamic_pointer_cast(readSFNode(COLOR_NODE)); break; case F_COORD: node->coord = dynamic_pointer_cast(readSFNode(COORDINATE_NODE)); break; case F_COLOR_INDEX: readMFInt32(node->colorIndex); break; case F_COLOR_PER_VERTEX: readSFBool(node->colorPerVertex); break; case F_COORD_INDEX: readMFInt32(node->coordIndex); break; case F_CCW: readSFBool(node->ccw); break; case F_CONVEX: readSFBool(node->convex); break; case F_SOLID: readSFBool(node->solid); break; case F_CREASE_ANGLE: readSFFloat(node->creaseAngle); break; case F_NORMAL_INDEX: readMFInt32(node->normalIndex); break; case F_NORMAL_PER_VERTEX: readSFBool(node->normalPerVertex); break; case F_TEX_COORD_INDEX: readMFInt32(node->texCoordIndex); break; case F_TEX_COORD: node->texCoord = dynamic_pointer_cast(readSFNode(TEXTURE_COORDINATE_NODE)); break; case F_NORMAL: node->normal = dynamic_pointer_cast(readSFNode(NORMAL_NODE)); break; default: scanner->throwException("Undefined field"); } } //checkIndexedFaceSet(node); return node; } void VrmlParserImpl::checkIndexedFaceSet(VrmlIndexedFaceSetPtr node) { MFInt32& index = node->coordIndex; MFVec3f& coord = node->coord->point; int numUsedVertices = 0; vector usedVertices(coord.size(), false); int n = index.size(); int i = 0; vector polygon; while(i < n){ polygon.resize(0); bool isSeparated = false; while(i < n){ if(index[i] < 0){ isSeparated = true; i++; break; } polygon.push_back(index[i]); if(!usedVertices[index[i]]){ usedVertices[index[i]] = true; numUsedVertices++; } i++; } const int numVertices = polygon.size(); if(numVertices < 3){ cerr << "Number of vertices is less than 3 !" << endl; } if(numVertices > 3){ cerr << "Polygon is not a triangle in "; cerr << scanner->filename << endl; for(int j=0; j < numVertices; j++){ cerr << polygon[j] << ","; } cerr << endl; } if(!isSeparated){ cerr << "Vertex index is not correctly separated by '-1'" << endl; } int n = coord.size(); for(int j=0; j < numVertices; j++){ if(polygon[j] >= n){ cerr << "index " << polygon[j] << " is over the number of vertices" << endl; } } bool isIndexOverlapped = false; bool isVertexOverlapped = false; for(int j = 0; j < numVertices - 1; j++){ for(int k = j+1; k < numVertices; k++){ if(polygon[j] == polygon[k]){ isIndexOverlapped = true; } SFVec3f& v1 = coord[polygon[j]]; SFVec3f& v2 = coord[polygon[k]]; if(v1 == v2){ isVertexOverlapped = true; } } } if(isIndexOverlapped){ cerr << "overlapped vertex index in one polygon: "; for(int l = 0; l < numVertices; l++){ cerr << polygon[l] << ","; } cerr << endl; } if(isVertexOverlapped){ cerr << "In " << scanner->filename << ":"; cerr << "two vertices in one polygon have the same position\n"; for(int l = 0; l < numVertices; l++){ SFVec3f& v = coord[polygon[l]]; cerr << polygon[l] << " = (" << v[0] << "," << v[1] << "," << v[2] << ") "; } cerr << endl; } } if(numUsedVertices < static_cast(coord.size())){ cerr << "There are vertices which are not used in" << scanner->filename << ".\n"; cerr << "Number of vertices is " << coord.size(); cerr << ", Number of used ones is " << numUsedVertices << endl; } } VrmlCoordinatePtr VrmlParserImpl::readCoordNode() { VrmlCoordinatePtr node(new VrmlCoordinate); if(scanner->readSymbol()){ if(scanner->symbolValue != F_POINT){ scanner->throwException("Undefined field"); } readMFVec3f(node->point); } return node; } VrmlTextureCoordinatePtr VrmlParserImpl::readTextureCoordinateNode() { VrmlTextureCoordinatePtr node(new VrmlTextureCoordinate); if(scanner->readSymbol()){ if(scanner->symbolValue != F_POINT){ scanner->throwException("Undefined field"); } readMFVec2f(node->point); } return node; } VrmlColorPtr VrmlParserImpl::readColorNode() { VrmlColorPtr node(new VrmlColor); if(scanner->readSymbol()){ if(scanner->symbolValue != F_COLOR){ scanner->throwException("Undefined field"); } readMFColor(node->color); } return node; } VrmlNormalPtr VrmlParserImpl::readNormalNode() { VrmlNormalPtr node(new VrmlNormal); if(scanner->readSymbol()){ if(scanner->symbolValue != F_VECTOR){ scanner->throwException("Undefined field"); } readMFVec3f(node->vector); } return node; } VrmlAppearancePtr VrmlParserImpl::readAppearanceNode() { VrmlAppearancePtr node(new VrmlAppearance); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_MATERIAL: node->material = dynamic_pointer_cast(readSFNode(MATERIAL_NODE)); break; case F_TEXTURE: node->texture = dynamic_pointer_cast(readSFNode(TEXTURE_NODE)); break; case F_TEXTURE_TRANSFORM: node->textureTransform = dynamic_pointer_cast(readSFNode(TEXTURE_TRANSFORM_NODE)); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlMaterialPtr VrmlParserImpl::readMaterialNode() { VrmlMaterialPtr node(new VrmlMaterial); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_AMBIENT_INTENSITY: readSFFloat(node->ambientIntensity); break; case F_DIFFUSE_COLOR: readSFColor(node->diffuseColor); break; case F_EMISSIVE_COLOR: readSFColor(node->emissiveColor); break; case F_SHININESS: readSFFloat(node->shininess); break; case F_SPECULAR_COLOR: readSFColor(node->specularColor); break; case F_TRANSPARANCY: readSFFloat(node->transparency); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlImageTexturePtr VrmlParserImpl::readImageTextureNode() { VrmlImageTexturePtr node = new VrmlImageTexture; while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_URL: readMFString(node->url); break; case F_REPEAT_S: readSFBool(node->repeatS); break; case F_REPEAT_T: readSFBool(node->repeatT); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlTextureTransformPtr VrmlParserImpl::readTextureTransformNode() { VrmlTextureTransformPtr node(new VrmlTextureTransform); while(scanner->readSymbol()){ switch(scanner->symbolValue){ case F_CENTER: readSFVec2f(node->center); break; case F_ROTATION: readSFFloat(node->rotation); break; case F_SCALE: readSFVec2f(node->scale); break; case F_TRANSLATION: readSFVec2f(node->translation); break; default: scanner->throwException("Undefined field"); } } return node; } VrmlVariantField& VrmlParserImpl::readProtoField(VrmlFieldTypeId fieldTypeId) { if(!currentProtoInstance){ scanner->throwException("cannot use proto field value here"); } scanner->readWordEx("illegal field"); TProtoFieldMap::iterator p = currentProtoInstance->fields.find(scanner->stringValue); if(p == currentProtoInstance->fields.end()){ string msg = "This field("; msg += scanner->stringValue +") does not exist in proto node"; scanner->throwException(msg); } if(p->second.which() != fieldTypeId){ scanner->throwException("Unmatched field type"); } return p->second; } void VrmlParserImpl::readSFInt32(SFInt32& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(SFINT32)); } else { out_value = scanner->readIntEx("illegal int value"); } } void VrmlParserImpl::readMFInt32(MFInt32& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(MFINT32)); } else { out_value.clear(); if(!scanner->readChar('[')){ out_value.push_back(scanner->readIntEx("illegal int value")); } else { while(!scanner->readChar(']')){ out_value.push_back(scanner->readIntEx("illegal int value")); } } } } void VrmlParserImpl::readSFFloat(SFFloat& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(SFFLOAT)); } else { out_value = scanner->readDoubleEx("illegal float value"); } } void VrmlParserImpl::readMFFloat(MFFloat& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(MFFLOAT)); } else { out_value.clear(); if(!scanner->readChar('[')){ out_value.push_back(scanner->readDoubleEx("illegal float value")); } else { while(!scanner->readChar(']')){ out_value.push_back(scanner->readDoubleEx("illegal float value")); } } } } static inline SFColor readSFColor(EasyScannerPtr& scanner) { SFColor c; for(int i=0; i < 3; ++i){ c[i] = static_cast(scanner->readDoubleEx("illegal color element")); } return c; } void VrmlParserImpl::readSFColor(SFColor& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(SFCOLOR)); } else { out_value = ::readSFColor(scanner); } } void VrmlParserImpl::readMFColor(MFColor& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(MFCOLOR)); } else { out_value.clear(); if(!scanner->readChar('[')){ out_value.push_back(::readSFColor(scanner)); } else { while(!scanner->readChar(']')){ out_value.push_back(::readSFColor(scanner)); } } } } void VrmlParserImpl::readSFString(SFString& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(SFSTRING)); } else { out_value = scanner->readQuotedStringEx("illegal string"); } } void VrmlParserImpl::readMFString(MFString& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(MFSTRING)); } else { out_value.clear(); if(!scanner->readChar('[')){ out_value.push_back(scanner->readQuotedStringEx("illegal string")); } else { while(!scanner->readChar(']')){ out_value.push_back(scanner->readQuotedStringEx("illegal string")); } } } } static inline SFVec2f readSFVec2f(EasyScannerPtr& scanner) { SFVec2f v; for(int i=0; i < 2; ++i){ v[i] = scanner->readDoubleEx("illegal vector element"); } return v; } void VrmlParserImpl::readSFVec2f(SFVec2f& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(SFVEC2F)); } else { out_value = ::readSFVec2f(scanner); } } void VrmlParserImpl::readMFVec2f(MFVec2f& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(MFVEC2F)); } else { out_value.clear(); if(!scanner->readChar('[')){ out_value.push_back(::readSFVec2f(scanner)); } else { while(!scanner->readChar(']')){ out_value.push_back(::readSFVec2f(scanner)); } } } } static inline SFVec3f readSFVec3f(EasyScannerPtr& scanner) { SFVec3f v; for(int i=0; i < 3; ++i){ v[i] = scanner->readDoubleEx("illegal vector element"); } return v; } void VrmlParserImpl::readSFVec3f(SFVec3f& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(SFVEC3F)); } else { out_value = ::readSFVec3f(scanner); } } void VrmlParserImpl::readMFVec3f(MFVec3f& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(MFVEC3F)); } else { out_value.clear(); if(!scanner->readChar('[')){ out_value.push_back(::readSFVec3f(scanner)); } else { while(!scanner->readChar(']')){ out_value.push_back(::readSFVec3f(scanner)); } } } } static inline SFRotation readSFRotation(EasyScannerPtr& scanner) { SFRotation rotation; SFRotation::Vector3& axis = rotation.axis(); for(int i=0; i < 3; ++i){ axis[i] = scanner->readDoubleEx("illegal rotaion axis element"); } rotation.angle() = scanner->readDoubleEx("illegal rotaion angle value"); double size = axis.norm(); if(size < 1.0e-6){ scanner->throwException("Rotation axis is the zero vector"); } axis /= size; // normalize return rotation; } void VrmlParserImpl::readSFRotation(SFRotation& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(SFROTATION)); } else { out_value = ::readSFRotation(scanner); } } void VrmlParserImpl::readMFRotation(MFRotation& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(MFROTATION)); } else { out_value.clear(); if(!scanner->readChar('[')){ out_value.push_back(::readSFRotation(scanner)); } else { while(!scanner->readChar(']')){ out_value.push_back(::readSFRotation(scanner)); } } } } void VrmlParserImpl::readSFBool(SFBool& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(SFBOOL)); } else { switch(scanner->readSymbolEx("no bool value")){ case V_TRUE: out_value = true; break; case V_FALSE: out_value = false; break; default: scanner->throwException("no bool value"); } } } void VrmlParserImpl::readSFImage(SFImage& out_image) { if(scanner->readSymbol(F_IS)){ out_image = get(readProtoField(SFIMAGE)); } else { readSFInt32(out_image.width); readSFInt32(out_image.height); readSFInt32(out_image.numComponents); //! start reading pixel values per component. //! numComponents means: //! 1:grayscale, 2:grayscale with transparency //! 3:RGB components, 4:RGB components with transparency SFInt32 pixelValue; unsigned char componentValue; const SFInt32 comps = out_image.numComponents; for(int h=0; h < out_image.height; ++h){ for(int w=0; w < out_image.width; ++w){ readSFInt32(pixelValue); for(int i=0, shift=8*(comps - 1); i < comps; ++i, shift-=8){ // get each component values from left 8 bytes componentValue = (unsigned char)((pixelValue >> shift) & 0x000000FF); out_image.pixels.push_back(componentValue); } } } } } void VrmlParserImpl::readSFTime(SFTime& out_value) { if(scanner->readSymbol( F_IS )){ out_value = get(readProtoField(SFTIME)); } else { out_value = scanner->readDoubleEx("illegal time value"); } } void VrmlParserImpl::readMFTime(MFTime& out_value) { if(scanner->readSymbol(F_IS)){ out_value = get(readProtoField(MFTIME)); } else { out_value.clear(); if(!scanner->readChar('[' )){ out_value.push_back(SFTime(scanner->readDoubleEx("illegal time value"))); } else { while(!scanner->readChar(']')){ out_value.push_back(SFTime(scanner->readDoubleEx("illegal time value"))); } } } } // This API should be obsolete void VrmlParserImpl::readSFNode(SFNode& out_node, VrmlNodeCategory nodeCategory) { if(scanner->readSymbol(F_IS)){ out_node = get(readProtoField(SFNODE)); } else if(scanner->readSymbol(V_NULL)){ out_node = 0; } else { out_node = readNode(nodeCategory); } } SFNode VrmlParserImpl::readSFNode(VrmlNodeCategory nodeCategory) { if(scanner->readSymbol(F_IS)){ return get(readProtoField(SFNODE)); } else if(scanner->readSymbol(V_NULL)){ return 0; } else { return readNode(nodeCategory); } } void VrmlParserImpl::readMFNode(MFNode& out_nodes, VrmlNodeCategory nodeCategory) { if(scanner->readSymbol(F_IS)){ out_nodes = get(readProtoField(MFNODE)); } else { SFNode sfnode; out_nodes.clear(); if(!scanner->readChar('[')){ readSFNode(sfnode, nodeCategory); out_nodes.push_back(sfnode); } else { while(true) { readSFNode(sfnode, nodeCategory); if(sfnode){ out_nodes.push_back(sfnode); } bool closed = scanner->readChar(']'); if(closed){ break; } if(!sfnode && !closed){ scanner->throwException("syntax error"); } } } } } void VrmlParserImpl::init() { currentProtoInstance = 0; protoInstanceActualNodeExtractionMode = true; scanner = boost::shared_ptr( new EasyScanner() ); setSymbols(); } void VrmlParserImpl::setSymbols() { struct TSymbol { int id; const char* symbol; }; static TSymbol symbols[] = { // values { V_TRUE, "TRUE" }, { V_FALSE, "FALSE" }, { V_NULL, "NULL" }, // types { T_SFINT32, "SFInt32" }, { T_MFINT32, "MFInt32" }, { T_SFFLOAT, "SFFloat" }, { T_MFFLOAT, "MFFloat" }, { T_SFVEC2F, "SFVec2f" }, { T_MFVEC2F, "MFVec2f" }, { T_SFVEC3F, "SFVec3f" }, { T_MFVEC3F, "MFVec3f" }, { T_SFROTATION, "SFRotation" }, { T_MFROTATION, "MFRotation" }, { T_SFTIME, "SFTime" }, { T_MFTIME, "MFTime" }, { T_SFCOLOR, "SFColor" }, { T_MFCOLOR, "MFColor" }, { T_SFSTRING, "SFString" }, { T_MFSTRING, "MFString" }, { T_SFNODE, "SFNode" }, { T_MFNODE, "MFNode" }, { T_SFBOOL, "SFBool" }, { T_SFIMAGE, "SFImage" }, // Nodes { N_PROTO, "PROTO" }, { N_INLINE, "Inline" }, { N_BACKGROUND, "Background" }, { N_NAVIGATION_INFO, "NavigationInfo" }, { N_VIEWPOINT, "Viewpoint" }, { N_GROUP, "Group" }, { N_TRANSFORM, "Transform" }, { N_SHAPE, "Shape" }, { N_APPEARANCE, "Appearance" }, { N_MATERIAL, "Material" }, { N_IMAGE_TEXTURE, "ImageTexture" }, { N_TEXTURE_TRANSFORM, "TextureTransform" }, { N_BOX, "Box" }, { N_CONE, "Cone" }, { N_CYLINDER, "Cylinder" }, { N_SPHERE, "Sphere" }, { N_TEXT, "Text" }, { N_FONT_STYLE, "FontStyle" }, { N_INDEXED_LINE_SET, "IndexedLineSet" }, { N_INDEXED_FACE_SET, "IndexedFaceSet" }, { N_COLOR, "Color" }, { N_COORDINATE, "Coordinate" }, { N_TEXTURE_COORDINATE, "TextureCoordinate" }, { N_NORMAL, "Normal" }, { N_CYLINDER_SENSOR, "CylinderSensor" }, { N_POINTSET, "PointSet" }, { N_PIXEL_TEXTURE,"PixelTexture" }, { N_MOVIE_TEXTURE, "MovieTexture" }, { N_ELEVATION_GRID, "ElevationGrid" }, { N_EXTRUSION, "Extrusion" }, { N_SWITCH, "Switch" }, { N_LOD,"LOD" }, { N_COLLISION, "Collision" }, { N_ANCHOR, "Anchor" }, { N_FOG, "Fog" }, { N_BILLBOARD, "Billboard" }, { N_WORLD_INFO, "WorldInfo" }, { N_POINT_LIGHT, "PointLight" }, { N_DIRECTIONAL_LIGHT, "DirectionalLight" }, { N_SPOT_LIGHT, "SpotLight" }, // unsupported nodes { N_AUDIO_CLIP, "AudioClip" }, { N_SOUND, "Sound" }, { N_COLOR_INTERPOLATOR, "ColorInterpolator" }, { N_COORDINATE_INTERPOLATOR, "CoordinateInterpolator" }, { N_ORIENTATION_INTERPOLATOR, "OrientationInterpolator" }, { N_NORMAL_INTERPOLATOR, "NormalInterpolator" }, { N_POSITION_INTERPOLATOR, "PositionInterpolator" }, { N_SCALAR_INTERPOLATOR, "ScalarInterpolator" }, { N_PLANE_SENSOR, "PlaneSensor" }, { N_PROXIMITY_SENSOR, "ProximitySensor" }, { N_SPHERE_SENSOR, "SphereSensor" }, { N_TIME_SENSOR, "TimeSensor" }, { N_TOUCH_SENSOR, "TouchSensor" }, { N_VISIBILITY_SENSOR, "VisibilitySensor" }, // Fields { F_IS, "IS" }, { F_URL, "url" }, { F_GROUND_ANGLE, "groundAngle" }, { F_GROUND_COLOR, "groundColor" }, { F_SKY_ANGLE, "skyAngle" }, { F_SKY_COLOR, "skyColor" }, { F_BACK_URL, "backUrl" }, { F_BOTTOM_URL, "bottomUrl" }, { F_FRONT_URL, "frontUrl" }, { F_LEFT_URL, "leftUrl" }, { F_RIGHT_URL, "rightUrl" }, { F_TOP_URL, "topUrl" }, { F_AVATAR_SIZE, "avatarSize" }, { F_HEADLIGHT, "headlight" }, { F_SPEED, "speed" }, { F_TYPE, "type" }, { F_VISIBILITY_LIMIT, "visibilityLimit" }, { F_FIELD_OF_VIEW, "fieldOfView" }, { F_JUMP, "jump" }, { F_ORIENTATION, "orientation" }, { F_POSITION, "position" }, { F_DESCRIPTION, "description" }, { F_CHILDREN, "children" }, { F_ADD_CHILDREN, "addChildren" }, { F_REMOVE_CHILDREN, "removeChildren" }, { F_BBOX_CENTER, "bboxCenter" }, { F_BBOX_SIZE, "bboxSize" }, { F_CENTER, "center" }, { F_ROTATION, "rotation" }, { F_SCALE, "scale" }, { F_SCALE_ORIENTATION, "scaleOrientation" }, { F_TRANSLATION, "translation" }, { F_APPEARANCE, "appearance" }, { F_GEOMETRY, "geometry" }, { F_MATERIAL, "material" }, { F_TEXTURE, "texture" }, { F_TEXTURE_TRANSFORM, "textureTransform" }, { F_AMBIENT_INTENSITY, "ambientIntensity" }, { F_DIFFUSE_COLOR, "diffuseColor" }, { F_EMISSIVE_COLOR, "emissiveColor" }, { F_SHININESS, "shininess" }, { F_SPECULAR_COLOR, "specularColor" }, { F_TRANSPARANCY, "transparency" }, { F_DIRECTION, "direction" }, { F_REPEAT_S, "repeatS" }, { F_REPEAT_T, "repeatT" }, { F_SIZE, "size" }, { F_BOTTOM, "bottom" }, { F_BOTTOM_RADIUS, "bottomRadius" }, { F_HEIGHT, "height" }, { F_SIDE, "side" }, { F_RADIUS, "radius" }, { F_TOP, "top" }, { F_STRING, "string" }, { F_FONT_STYLE, "fontStyle" }, { F_LENGTH, "length" }, { F_MAX_EXTENT, "maxExtent" }, { F_FAMILY, "family" }, { F_HORIZONTAL, "horizontal" }, { F_JUSTIFY, "justify" }, { F_LANGUAGE, "language" }, { F_LEFT_TO_RIGHT, "leftToRight" }, { F_SPACING, "spacing" }, { F_STYLE, "style" }, { F_TOP_TO_BOTTOM, "topToBottom" }, { F_COLOR, "color" }, { F_COORD, "coord" }, { F_COLOR_INDEX, "colorIndex" }, { F_COLOR_PER_VERTEX, "colorPerVertex" }, { F_COORD_INDEX, "coordIndex" }, { F_CCW, "ccw" }, { F_CONVEX, "convex" }, { F_SOLID, "solid" }, { F_CREASE_ANGLE, "creaseAngle" }, { F_NORMAL_INDEX, "normalIndex" }, { F_NORMAL, "normal" }, { F_NORMAL_PER_VERTEX, "normalPerVertex" }, { F_TEX_COORD_INDEX, "texCoordIndex" }, { F_TEX_COORD, "texCoord" }, { F_POINT, "point" }, { F_VECTOR, "vector" }, { F_AUTO_OFFSET, "autoOffset" }, { F_DISK_ANGLE, "diskAngle" }, { F_ENABLED, "enabled" }, { F_MAX_ANGLE, "maxAngle" }, { F_MIN_ANGLE, "minAngle" }, { F_OFFSET, "offset" }, { F_IMAGE, "image" }, { F_X_DIMENSION, "xDimension" }, { F_Z_DIMENSION, "zDimension" }, { F_X_SPACING, "xSpacing" }, { F_Z_SPACING, "zSpacing" }, { F_CROSS_SECTION, "crossSection" }, { F_SPINE, "spine" }, { F_BEGIN_CAP, "beginCap" }, { F_END_CAP, "endCap" }, { F_CHOICE, "choice" }, { F_WHICH_CHOICE, "whichChoice" }, { F_COLLIDE, "collide" }, { F_PROXY, "proxy" }, { F_PARAMETER, "parameter" }, { F_VISIBILITY_RANGE, "visibilityRange" }, { F_FOG_TYPE, "fogType" }, { F_AXIS_OF_ROTATION, "axisOfRotation" }, { F_TITLE, "title" }, { F_INFO, "info" }, // event type { E_FIELD, "field" }, { E_EXPOSED_FIELD, "exposedField" }, { E_EVENTIN, "eventIn" }, { E_EVENTOUT, "eventOut" }, // def & route { D_DEF, "DEF" }, { D_USE, "USE" }, { D_ROUTE, "ROUTE" }, // unsupported keywords { U_SCRIPT, "Script" }, { U_EXTERNPROTO, "EXTERNPROTO" }, { 0, "" } }; for(int i=0; symbols[i].id != 0; i++){ scanner->registerSymbol(symbols[i].id, symbols[i].symbol); } } choreonoid-1.1.0+dfsg/src/Util/VrmlParser.h000066400000000000000000000020151207742442300205360ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_VRML_PARSER_H_INCLUDED #define CNOID_UTIL_VRML_PARSER_H_INCLUDED #include "VrmlNodes.h" #include "exportdecl.h" namespace cnoid { class VrmlParserImpl; /** \brief Parser for VRML97 format The VrmlParser class reads a VRML97 file and extract its nodes. */ class CNOID_EXPORT VrmlParser { public: /** Constructor. This version of constructor do 'load' mehtod after constructing the object. \param filename file name of a target VRML97 file. */ VrmlParser(const std::string& filename); VrmlParser(); ~VrmlParser(); void setProtoInstanceActualNodeExtractionMode(bool isOn); void load(const std::string& filename); /** This method returns the top node of the next node tree written in the file. */ VrmlNodePtr readNode(); private: VrmlParserImpl* impl; void init(); }; }; #endif choreonoid-1.1.0+dfsg/src/Util/VrmlWriter.cpp000066400000000000000000000163561207742442300211260ustar00rootroot00000000000000/*! @file @author Shin'ichiro Nakaoka */ #include "VrmlWriter.h" using namespace std; using namespace boost; using namespace cnoid; namespace { typedef void (VrmlWriter::*VrmlWriterNodeMethod)(VrmlNodePtr node); typedef std::map TNodeMethodMap; typedef std::pair TNodeMethodPair; TNodeMethodMap nodeMethodMap; inline void registerNodeMethod(const std::type_info& t, VrmlWriterNodeMethod method) { nodeMethodMap.insert(TNodeMethodPair(t.name(), method)); } VrmlWriterNodeMethod getNodeMethod(VrmlNodePtr node) { TNodeMethodMap::iterator p = nodeMethodMap.find(typeid(*node).name()); return (p != nodeMethodMap.end()) ? p->second : 0; } inline std::ostream& operator<<(std::ostream& out, VrmlWriter::TIndent& indent) { return out << indent.spaces; } inline const char* boolstr(bool v) { if(v){ return "TRUE"; } else { return "FALSE"; } } ostream& operator<<(std::ostream& out, const SFVec2f& v) { return out << v[0] << " " << v[1]; } ostream& operator<<(std::ostream& out, const SFVec3f& v) { return out << v[0] << " " << v[1] << " " << v[2]; } ostream& operator<<(std::ostream& out, const SFColor& v) { return out << v[0] << " " << v[1] << " " << v[2]; } ostream& operator<<(std::ostream& out, const SFRotation& v) { const SFRotation::Vector3& a = v.axis(); return out << a[0] << " " << a[1] << " " << a[2] << " " << v.angle(); } } template void VrmlWriter::writeMFValues(MFValues values, int numColumn) { out << ++indent << "[\n"; ++indent; out << indent; int col = 0; int n = values.size(); for(int i=0; i < n; i++){ out << values[i] << " "; col++; if(col == numColumn){ col = 0; out << "\n"; if(i < n-1){ out << indent; } } } out << --indent << "]\n"; --indent; } void VrmlWriter::writeMFInt32SeparatedByMinusValue(MFInt32& values) { out << ++indent << "[\n"; ++indent; out << indent; int n = values.size(); for(int i=0; i < n; i++){ out << values[i] << " "; if(values[i] < 0){ out << "\n"; if(i < n-1){ out << indent; } } } out << --indent << "]\n"; --indent; } VrmlWriter::VrmlWriter(std::ostream& out) : out(out) { if(nodeMethodMap.empty()){ registerNodeMethodMap(); } } void VrmlWriter::registerNodeMethodMap() { registerNodeMethod(typeid(VrmlGroup), &VrmlWriter::writeGroupNode); registerNodeMethod(typeid(VrmlTransform), &VrmlWriter::writeTransformNode); registerNodeMethod(typeid(VrmlShape), &VrmlWriter::writeShapeNode); registerNodeMethod(typeid(VrmlIndexedFaceSet), &VrmlWriter::writeIndexedFaceSetNode); } void VrmlWriter::writeHeader() { out << "#VRML V2.0 utf8\n"; } bool VrmlWriter::writeNode(VrmlNodePtr node) { indent.clear(); out << "\n"; writeNodeIter(node); return true; } void VrmlWriter::writeNodeIter(VrmlNodePtr node) { VrmlWriterNodeMethod method = getNodeMethod(node); if(method){ (this->*method)(node); } } void VrmlWriter::beginNode(const char* nodename, VrmlNodePtr node) { out << indent; if(node->defName.empty()){ out << nodename << " {\n"; } else { out << "DEF " << node->defName << " " << nodename << " {\n"; } ++indent; } void VrmlWriter::endNode() { out << --indent << "}\n"; } void VrmlWriter::writeGroupNode(VrmlNodePtr node) { VrmlGroupPtr group = static_pointer_cast(node); beginNode("Group", group); writeGroupFields(group); endNode(); } void VrmlWriter::writeGroupFields(VrmlGroupPtr group) { if(group->bboxSize[0] >= 0){ out << indent << "bboxCenter " << group->bboxCenter << "\n"; out << indent << "bboxSize " << group->bboxSize << "\n"; } if(!group->children.empty()){ out << indent << "children [\n"; ++indent; for(size_t i=0; i < group->children.size(); i++){ writeNodeIter(group->children[i]); } out << --indent << "]\n"; } } void VrmlWriter::writeTransformNode(VrmlNodePtr node) { VrmlTransformPtr trans = static_pointer_cast(node); beginNode("Transform", trans); out << indent << "center " << trans->center << "\n"; out << indent << "rotation " << trans->rotation << "\n"; out << indent << "scale " << trans->scale << "\n"; out << indent << "scaleOrientation " << trans->scaleOrientation << "\n"; out << indent << "translation " << trans->translation << "\n"; writeGroupFields(trans); endNode(); } void VrmlWriter::writeShapeNode(VrmlNodePtr node) { VrmlShapePtr shape = static_pointer_cast(node); beginNode("Shape", shape); if(shape->appearance){ out << indent << "appearance\n"; ++indent; writeAppearanceNode(shape->appearance); --indent; } if(shape->geometry){ out << indent << "geometry\n"; VrmlWriterNodeMethod method = getNodeMethod(shape->geometry); if(method){ ++indent; (this->*method)(shape->geometry); --indent; } } endNode(); } void VrmlWriter::writeAppearanceNode(VrmlAppearancePtr appearance) { beginNode("Appearance", appearance); if(appearance->material){ out << indent << "material\n"; ++indent; writeMaterialNode(appearance->material); --indent; } endNode(); } void VrmlWriter::writeMaterialNode(VrmlMaterialPtr material) { beginNode("Material", material); out << indent << "ambientIntensity " << material->ambientIntensity << "\n"; out << indent << "diffuseColor " << material->diffuseColor << "\n"; out << indent << "emissiveColor " << material->emissiveColor << "\n"; out << indent << "shininess " << material->shininess << "\n"; out << indent << "specularColor " << material->specularColor << "\n"; out << indent << "transparency " << material->transparency << "\n"; endNode(); } void VrmlWriter::writeIndexedFaceSetNode(VrmlNodePtr node) { VrmlIndexedFaceSetPtr faceset = static_pointer_cast(node); beginNode("IndexedFaceSet", faceset); if(faceset->coord){ out << indent << "coord\n"; ++indent; writeCoordinateNode(faceset->coord); --indent; } if(!faceset->coordIndex.empty()){ out << indent << "coordIndex\n"; writeMFInt32SeparatedByMinusValue(faceset->coordIndex); } out << indent << "ccw " << boolstr(faceset->ccw) << "\n"; out << indent << "convex " << boolstr(faceset->convex) << "\n"; out << indent << "creaseAngle " << faceset->creaseAngle << "\n"; out << indent << "solid " << boolstr(faceset->solid) << "\n"; endNode(); } void VrmlWriter::writeCoordinateNode(VrmlCoordinatePtr coord) { beginNode("Coordinate", coord); if(!coord->point.empty()){ out << indent << "point\n"; writeMFValues(coord->point, 1); } endNode(); } choreonoid-1.1.0+dfsg/src/Util/VrmlWriter.h000066400000000000000000000026521207742442300205650ustar00rootroot00000000000000 /*! @file @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_VRML_WRITER_INCLUDED #define CNOID_UTIL_VRML_WRITER_INCLUDED #include "VrmlNodes.h" #include #include #include #include "exportdecl.h" namespace cnoid { class VrmlWriter; class CNOID_EXPORT VrmlWriter { public: VrmlWriter(std::ostream& out); void writeHeader(); bool writeNode(VrmlNodePtr node); struct TIndent { void clear() { n = 0; spaces.resize(n); } inline TIndent& operator++() { n += 2; spaces.resize(n, ' '); return *this; } inline TIndent& operator--() { n -= 2; if(n < 0) { n = 0; } spaces.resize(n, ' '); return *this; } std::string spaces; int n; }; private: std::ostream& out; TIndent indent; void registerNodeMethodMap(); template void writeMFValues(MFValues values, int numColumn); void writeMFInt32SeparatedByMinusValue(MFInt32& values); void writeNodeIter(VrmlNodePtr node); void beginNode(const char* nodename, VrmlNodePtr node); void endNode(); void writeGroupNode(VrmlNodePtr node); void writeGroupFields(VrmlGroupPtr group); void writeTransformNode(VrmlNodePtr node); void writeShapeNode(VrmlNodePtr node); void writeAppearanceNode(VrmlAppearancePtr appearance); void writeMaterialNode(VrmlMaterialPtr material); void writeIndexedFaceSetNode(VrmlNodePtr node); void writeCoordinateNode(VrmlCoordinatePtr coord); }; }; #endif choreonoid-1.1.0+dfsg/src/Util/YamlNodes.cpp000066400000000000000000000406471207742442300207040ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "YamlNodes.h" #include #include #include #include #include #include #ifdef _WIN32 #define snprintf _snprintf_s #endif using namespace std; using namespace boost; using namespace cnoid; namespace { const bool debugTrace = false; const char* typeNames[] = { "unknown node", "mapping", "sequence", "scalar" }; map booleanSymbols; YamlNodePtr invalidNode; YamlMappingPtr invalidMapping; YamlSequencePtr invalidSequence; } YamlNode::Exception::~Exception() { } // disabled YamlNode::YamlNode(const YamlNode&) { // throw an exception here ? } // disabled YamlNode& YamlNode::operator=(const YamlNode&) { // throw an exception here ? return *this; } void YamlNode::initialize() { static bool initialized = false; if(!initialized){ invalidNode = new YamlNode(YAML_NONE); invalidMapping = new YamlMapping(); invalidMapping->type_ = YAML_NONE; invalidSequence = new YamlSequence(); invalidSequence->type_ = YAML_NONE; booleanSymbols["true"] = true; booleanSymbols["yes"] = true; booleanSymbols["on"] = true; booleanSymbols["false"] = false; booleanSymbols["no"] = false; booleanSymbols["off"] = false; initialized = true; } } bool YamlNode::read(int &out_value) const { if(type_ == YAML_SCALAR){ const char* nptr = &(static_cast(this)->stringValue[0]); char* endptr; out_value = strtol(nptr, &endptr, 10); if(endptr > nptr){ return true; } } return false; } int YamlNode::toInt() const { if(type_ != YAML_SCALAR){ throwNotScalrException(); } const YamlScalar* const scalar = static_cast(this); const char* nptr = &(scalar->stringValue[0]); char* endptr; const int value = strtol(nptr, &endptr, 10); if(endptr == nptr){ ScalarTypeMismatchException ex; ex.setMessage(str(format("\"%1%\" at line %2%, column %3% should be an integer value.") % scalar->stringValue % line() % column())); ex.setPosition(line(), column()); throw ex; } return value; } double YamlNode::toDouble() const { if(type_ != YAML_SCALAR){ throwNotScalrException(); } const YamlScalar* const scalar = static_cast(this); const char* nptr = &(scalar->stringValue[0]); char* endptr; const double value = strtod(nptr, &endptr); if(endptr == nptr){ ScalarTypeMismatchException ex; ex.setMessage(str(format("\"%1%\" at line %2%, column %3% should be a double value.") % scalar->stringValue % line() % column())); ex.setPosition(line(), column()); throw ex; } return value; } bool YamlNode::toBool() const { if(type_ != YAML_SCALAR){ throwNotScalrException(); } const YamlScalar* const scalar = static_cast(this); map::iterator p = booleanSymbols.find(scalar->stringValue); if(p != booleanSymbols.end()){ return p->second; } ScalarTypeMismatchException ex; ex.setMessage(str(format("\"%1%\" at line %2%, column %3% should be a bool value.") % scalar->stringValue % line() % column())); ex.setPosition(line(), column()); throw ex; } #ifdef _WIN32 const std::string YamlNode::toString() const { if(type_ != YAML_SCALAR){ throwNotScalrException(); } return fromUtf8(static_cast(this)->stringValue); } const std::string YamlNode::toUtf8String() const { if(type_ != YAML_SCALAR){ throwNotScalrException(); } return static_cast(this)->stringValue; } #else const std::string& YamlNode::toString() const { if(type_ != YAML_SCALAR){ throwNotScalrException(); } return static_cast(this)->stringValue; } const std::string& YamlNode::toUtf8String() const { return toString(); } #endif YamlScalar::YamlScalar(const std::string& value, YamlStringStyle stringStyle) : stringValue(value), stringStyle(stringStyle) { type_ = YAML_SCALAR; line_ = -1; column_ = -1; } YamlScalar::YamlScalar(const char* text, size_t length) : stringValue(text, length) { type_ = YAML_SCALAR; stringStyle = YAML_PLAIN_STRING; } YamlScalar::YamlScalar(const char* text, size_t length, YamlStringStyle stringStyle) : stringValue(text, length), stringStyle(stringStyle) { type_ = YAML_SCALAR; line_ = -1; column_ = -1; } const YamlMapping* YamlNode::toMapping() const { if(type_ != YAML_MAPPING){ throwNotMappingException(); } return static_cast(this); } YamlMapping* YamlNode::toMapping() { if(type_ != YAML_MAPPING){ throwNotMappingException(); } return static_cast(this); } const YamlSequence* YamlNode::toSequence() const { if(type_ != YAML_SEQUENCE){ throwNotSequenceException(); } return static_cast(this); } YamlSequence* YamlNode::toSequence() { if(type_ != YAML_SEQUENCE){ throwNotSequenceException(); } return static_cast(this); } void YamlNode::throwNotScalrException() const { NotScalarException ex; if(hasLineInfo()){ ex.setMessage(str(format("The %1% at line %2%, column %3% should be a scalar value.") % typeNames[type_] % line() % column())); } else { ex.setMessage("Scalar value cannot be obtained from a non-scalar type yaml node."); } ex.setPosition(line(), column()); throw ex; } void YamlNode::throwNotMappingException() const { NotMappingException ex; ex.setPosition(line(), column()); throw ex; } void YamlNode::throwNotSequenceException() const { NotSequenceException ex; ex.setPosition(line(), column()); throw ex; } YamlCollection::YamlCollection() { isFlowStyle_ = false; static const char* defaultFormat = "%.6g"; doubleFormat_ = defaultFormat; } YamlCollection::~YamlCollection() { } void YamlCollection::setDoubleFormat(const char* format) { doubleFormat_ = format; } YamlMapping::YamlMapping() { type_ = YAML_MAPPING; line_ = -1; column_ = -1; mode = READ_MODE; indexCounter = 0; keyQuoteStyle = YAML_PLAIN_STRING; } YamlMapping::YamlMapping(int line, int column) { type_ = YAML_MAPPING; line_ = line; column_ = column; mode = READ_MODE; indexCounter = 0; } YamlMapping::~YamlMapping() { clear(); } void YamlMapping::clear() { values.clear(); indexCounter = 0; } void YamlMapping::setKeyQuoteStyle(YamlStringStyle style) { keyQuoteStyle = style; } YamlNode* YamlMapping::find(const std::string& key) const { if(!isValid()){ throwNotMappingException(); } const_iterator p = values.find(toUtf8(key)); if(p != values.end()){ return p->second.get(); } else { return invalidNode.get(); } } YamlMapping* YamlMapping::findMapping(const std::string& key) const { if(!isValid()){ throwNotMappingException(); } const_iterator p = values.find(toUtf8(key)); if(p != values.end()){ YamlNode* node = p->second.get(); if(node->type() == YAML_MAPPING){ return static_cast(node); } } return invalidMapping.get(); } YamlSequence* YamlMapping::findSequence(const std::string& key) const { if(!isValid()){ throwNotMappingException(); } const_iterator p = values.find(toUtf8(key)); if(p != values.end()){ YamlNode* node = p->second.get(); if(node->type() == YAML_SEQUENCE){ return static_cast(node); } } return invalidSequence.get(); } YamlNode& YamlMapping::get(const std::string& key) const { if(!isValid()){ throwNotMappingException(); } const_iterator p = values.find(toUtf8(key)); if(p == values.end()){ throwKeyNotFoundException(key); } return *p->second; } void YamlMapping::throwKeyNotFoundException(const std::string& key) const { KeyNotFoundException ex; ex.setMessage(str(format("Key \"%1%\" is not found in the mapping that begins at line %2%, column %3%.") % key % line() % column())); ex.setPosition(line(), column()); ex.setKey(key); throw ex; } inline void YamlMapping::insertSub(const std::string& key, YamlNode* node) { values.insert(make_pair(key, node)); node->indexInMapping = indexCounter++; } void YamlMapping::insert(const std::string& key, YamlNodePtr node) { if(!isValid()){ throwNotMappingException(); } const string uKey(toUtf8(key)); iterator p = values.find(uKey); if(p != values.end()){ values.erase(p); } insertSub(uKey, node.get()); } YamlMapping* YamlMapping::openMapping(const std::string& key, bool doOverwrite) { if(!isValid()){ throwNotMappingException(); } YamlMapping* mapping = 0; const string uKey(toUtf8(key)); iterator p = values.find(uKey); if(p != values.end()){ YamlNode* node = p->second.get(); if(node->type() != YAML_MAPPING){ values.erase(p); } else { mapping = static_cast(node); if(doOverwrite){ mapping->clear(); } mapping->indexInMapping = indexCounter++; } } if(!mapping){ mapping = new YamlMapping(); mapping->doubleFormat_ = doubleFormat_; insertSub(uKey, mapping); } return mapping; } YamlMapping* YamlMapping::openFlowStyleMapping(const std::string& key, bool doOverwrite) { YamlMapping* m = openMapping(key, doOverwrite); m->setFlowStyle(true); return m; } YamlSequence* YamlMapping::openSequence(const std::string& key, bool doOverwrite) { if(!isValid()){ throwNotMappingException(); } YamlSequence* sequence = 0; const string uKey(toUtf8(key)); iterator p = values.find(uKey); if(p != values.end()){ YamlNode* node = p->second.get(); if(node->type() != YAML_SEQUENCE){ values.erase(p); } else { sequence = static_cast(node); if(doOverwrite){ sequence->clear(); } sequence->indexInMapping = indexCounter++; } } if(!sequence){ sequence = new YamlSequence(); sequence->doubleFormat_ = doubleFormat_; insertSub(uKey, sequence); } return sequence; } YamlSequence* YamlMapping::openFlowStyleSequence(const std::string& key, bool doOverwrite) { YamlSequence* s = openSequence(key, doOverwrite); s->setFlowStyle(true); return s; } bool YamlMapping::read(const std::string &key, std::string &out_value) const { YamlNode* node = find(toUtf8(key)); if(node->isValid()){ out_value = node->toString(); return true; } return false; } bool YamlMapping::readUtf8(const std::string &key, std::string &out_value) const { YamlNode* node = find(toUtf8(key)); if(node->isValid()){ out_value = node->toUtf8String(); return true; } return false; } bool YamlMapping::read(const std::string &key, bool &out_value) const { YamlNode* node = find(toUtf8(key)); if(node->isValid()){ out_value = node->toBool(); return true; } return false; } bool YamlMapping::read(const std::string &key, int &out_value) const { YamlNode* node = find(toUtf8(key)); if(node->isValid()){ out_value = node->toInt(); return true; } return false; } bool YamlMapping::read(const std::string &key, double &out_value) const { YamlNode* node = find(toUtf8(key)); if(node->isValid()){ out_value = node->toDouble(); return true; } return false; } void YamlMapping::writeUtf8(const std::string &key, const std::string& value, YamlStringStyle stringStyle) { string uKey(toUtf8(key)); iterator p = values.find(uKey); if(p == values.end()){ insertSub(uKey, new YamlScalar(value, stringStyle)); } else { YamlNode* node = p->second.get(); if(node->type() == YAML_SCALAR){ YamlScalar* scalar = static_cast(node); scalar->stringValue = value; scalar->stringStyle = stringStyle; scalar->indexInMapping = indexCounter++; } else { throwNotScalrException(); } } } /** This is for internal use. Text are not converted to UTF-8. */ void YamlMapping::writeSub(const std::string &key, const char* text, size_t length, YamlStringStyle stringStyle) { const string uKey(toUtf8(key)); iterator p = values.find(uKey); if(p == values.end()){ insertSub(uKey, new YamlScalar(text, length, stringStyle)); } else { YamlNode* node = p->second.get(); if(node->type() == YAML_SCALAR){ YamlScalar* scalar = static_cast(node); scalar->stringValue = string(text, length); scalar->stringStyle = stringStyle; scalar->indexInMapping = indexCounter++; } else { throwNotScalrException(); } } } void YamlMapping::write(const std::string &key, bool value) { if(value){ writeSub(key, "true", 4, YAML_PLAIN_STRING); } else { writeSub(key, "false", 5, YAML_PLAIN_STRING); } } void YamlMapping::write(const std::string &key, int value) { char buf[32]; int n = snprintf(buf, 32, "%d", value); writeSub(key, buf, n, YAML_PLAIN_STRING); } void YamlMapping::write(const std::string &key, double value) { char buf[32]; int n = snprintf(buf, 32, doubleFormat_, value); writeSub(key, buf, n, YAML_PLAIN_STRING); } void YamlMapping::writePath(const std::string &key, const std::string& value) { write(key, filesystem::path(value).string(), YAML_DOUBLE_QUOTED); } bool YamlMapping::compareIters(const YamlMapping::const_iterator& it1, const YamlMapping::const_iterator& it2) { return (it1->second->indexInMapping < it2->second->indexInMapping); } YamlSequence::YamlSequence() { type_ = YAML_SEQUENCE; line_ = -1; column_ = -1; } YamlSequence::YamlSequence(int size) : values(size) { type_ = YAML_SEQUENCE; line_ = -1; column_ = -1; } YamlSequence::YamlSequence(int line, int column) { type_ = YAML_SEQUENCE; line_ = line; column_ = column; } YamlSequence::YamlSequence(int line, int column, int reservedSize) : values(reservedSize) { type_ = YAML_SEQUENCE; line_ = line; column_ = column; values.resize(0); } YamlSequence::~YamlSequence() { clear(); } void YamlSequence::clear() { values.clear(); } void YamlSequence::reserve(int size) { values.reserve(size); } void YamlSequence::insertLF(int maxColumns, int numValues) { if(values.empty()){ if(numValues > 0 && numValues > maxColumns){ appendLF(); } } else if((values.size() % maxColumns) == 0){ appendLF(); } } YamlMapping* YamlSequence::newMapping() { YamlMapping* mapping = new YamlMapping(); mapping->doubleFormat_ = doubleFormat_; append(mapping); return mapping; } void YamlSequence::append(int value) { char buf[32]; int n = snprintf(buf, 32, "%d", value); values.push_back(new YamlScalar(buf, n, YAML_PLAIN_STRING)); } void YamlSequence::write(int i, int value) { char buf[32]; int n = snprintf(buf, 32, "%d", value); values[i] = new YamlScalar(buf, n, YAML_PLAIN_STRING); } void YamlSequence::append(size_t value) { char buf[32]; int n = snprintf(buf, 32, "%d", value); values.push_back(new YamlScalar(buf, n, YAML_PLAIN_STRING)); } void YamlSequence::append(double value) { char buf[32]; int n = snprintf(buf, 32, doubleFormat_, value); values.push_back(new YamlScalar(buf, n, YAML_PLAIN_STRING)); } void YamlSequence::append(const std::string& value, YamlStringStyle stringStyle) { values.push_back(new YamlScalar(toUtf8(value), stringStyle)); } void YamlSequence::write(int i, const std::string& value, YamlStringStyle stringStyle) { values[i] = new YamlScalar(toUtf8(value), stringStyle); } void YamlSequence::appendLF() { values.push_back(new YamlNode(YAML_LF)); } choreonoid-1.1.0+dfsg/src/Util/YamlNodes.h000066400000000000000000000375331207742442300203510ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_YAML_NODES_H_INCLUDED #define CNOID_UTIL_YAML_NODES_H_INCLUDED #include "Utf8.h" #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class YamlNode; } namespace boost{ void intrusive_ptr_add_ref(cnoid::YamlNode* obj); void intrusive_ptr_release(cnoid::YamlNode* obj); } namespace cnoid { enum YamlNodeType { YAML_NONE = 0, YAML_MAPPING, YAML_SEQUENCE, YAML_SCALAR, YAML_LF }; enum YamlStringStyle { YAML_PLAIN_STRING, YAML_SINGLE_QUOTED, YAML_DOUBLE_QUOTED, YAML_LITERAL, YAML_FOLDED }; class YamlScalar; class YamlMapping; class YamlSequence; class YamlReaderImpl; class YamlWriter; class CNOID_EXPORT YamlNode { public: static void initialize(); inline bool isValid() const { return type_ != YAML_NONE; } inline YamlNodeType type() const { return type_; } int toInt() const; double toDouble() const; bool toBool() const; inline bool isString() const { return type_ == YAML_SCALAR; } #ifdef _WIN32 const std::string toString() const; const std::string toUtf8String() const; inline operator std::string () const { return toString(); } #else const std::string& toString() const; const std::string& toUtf8String() const; inline operator const std::string& () const { return toString(); } #endif inline bool isMapping() const { return type_ == YAML_MAPPING; } const YamlMapping* toMapping() const; YamlMapping* toMapping(); inline bool isSequence() const { return type_ == YAML_SEQUENCE; } const YamlSequence* toSequence() const; YamlSequence* toSequence(); //bool read(std::string &out_value) const; //bool read(bool &out_value) const; bool read(int &out_value) const; //bool read(double &out_value) const; inline bool hasLineInfo() const { return (line_ >= 0); } inline int line() const { return line_ + 1; } inline int column() const { return column_ + 1; } class CNOID_EXPORT Exception { public: virtual ~Exception(); int line() const { return line_; } int column() const { return column_; } const std::string& message() const { return message_; } void setPosition(int line, int column) { line_ = line; column_ = column; } void setMessage(const std::string& m){ message_ = m; } private: int line_; int column_; std::string message_; }; class KeyNotFoundException : public Exception { public: const std::string& key() { return key_; } void setKey(const std::string& key) { key_ = key; } private: std::string key_; }; class NotScalarException : public Exception { }; class ScalarTypeMismatchException : public Exception { }; class NotMappingException : public Exception { }; class NotSequenceException : public Exception { }; class SyntaxException : public Exception { }; class DocumentNotFoundException : public Exception { }; private: int refCounter; protected: YamlNode() : refCounter(0) { } YamlNode(YamlNodeType type) : refCounter(0), type_(type), line_(-1), column_(-1) { } virtual ~YamlNode() { } void throwNotScalrException() const; void throwNotMappingException() const; void throwNotSequenceException() const; YamlNodeType type_; private: // disabled copy operations YamlNode(const YamlNode&); YamlNode& operator=(const YamlNode&); int line_; int column_; int indexInMapping; // used for YamlWriter friend class YamlReaderImpl; friend class YamlWriter; friend class YamlScalar; friend class YamlMapping; friend class YamlSequence; friend void boost::intrusive_ptr_add_ref(YamlNode* obj); friend void boost::intrusive_ptr_release(YamlNode* obj); }; typedef boost::intrusive_ptr YamlNodePtr; class CNOID_EXPORT YamlScalar : public YamlNode { private: YamlScalar(const char* text, size_t length); YamlScalar(const char* text, size_t length, YamlStringStyle stringStyle); YamlScalar(const std::string& value, YamlStringStyle stringStyle); std::string stringValue; YamlStringStyle stringStyle; friend class YamlReaderImpl; friend class YamlWriter; friend class YamlNode; friend class YamlMapping; friend class YamlSequence; }; class CNOID_EXPORT YamlCollection : public YamlNode { public: virtual ~YamlCollection(); inline void setFlowStyle(bool isFlowStyle = true) { isFlowStyle_ = isFlowStyle; } inline bool isFlowStyle() const { return isFlowStyle_; } void setDoubleFormat(const char* format); inline const char* doubleFormat() { return doubleFormat_; } protected: YamlCollection(); const char* doubleFormat_; private: YamlCollection(const YamlCollection&); YamlCollection& operator=(const YamlCollection&); bool isFlowStyle_; }; class CNOID_EXPORT YamlMapping : public YamlCollection { typedef std::map Container; public: typedef Container::iterator iterator; typedef Container::const_iterator const_iterator; YamlMapping(); YamlMapping(int line, int column); virtual ~YamlMapping(); inline bool empty() const { return values.empty(); } inline size_t size() const { return values.size(); } void clear(); void setKeyQuoteStyle(YamlStringStyle style); YamlNode* find(const std::string& key) const; YamlMapping* findMapping(const std::string& key) const; YamlSequence* findSequence(const std::string& key) const; YamlNode& get(const std::string& key) const; inline YamlNode& operator[](const std::string& key) const { return get(key); } void insert(const std::string& key, YamlNodePtr node); inline YamlMapping* openMapping(const std::string& key) { return openMapping(key, false); } inline YamlMapping* openFlowStyleMapping(const std::string& key) { return openFlowStyleMapping(key, false); } inline YamlMapping* createMapping(const std::string& key) { return openMapping(key, true); } inline YamlMapping* createFlowStyleMapping(const std::string& key) { return openFlowStyleMapping(key, true); } inline YamlSequence* openSequence(const std::string& key) { return openSequence(key, false); } inline YamlSequence* openFlowStyleSequence(const std::string& key){ return openFlowStyleSequence(key, false); } inline YamlSequence* createSequence(const std::string& key){ return openSequence(key, true); } inline YamlSequence* createFlowStyleSequence(const std::string& key){ return openFlowStyleSequence(key, true); } bool read(const std::string &key, std::string &out_value) const; bool readUtf8(const std::string &key, std::string &out_value) const; bool read(const std::string &key, bool &out_value) const; bool read(const std::string &key, int &out_value) const; bool read(const std::string &key, double &out_value) const; template inline T read(const std::string& key) const { T value; if(read(key, value)){ return value; } else { throwKeyNotFoundException(key); } } template inline T get(const std::string& key, const T& defaultValue) const { T value; if(read(key, value)){ return value; } else { return defaultValue; } } inline std::string get(const std::string& key, const char* defaultValue) const { std::string value; if(read(key, value)){ return value; } else { return defaultValue; } } void writeUtf8(const std::string &key, const std::string& value, YamlStringStyle stringStyle = YAML_PLAIN_STRING); void write(const std::string &key, const std::string& value, YamlStringStyle stringStyle = YAML_PLAIN_STRING) { writeUtf8(key, toUtf8(value), stringStyle); } void writeUtf8(const std::string &key, const char* value, YamlStringStyle stringStyle = YAML_PLAIN_STRING){ writeUtf8(key, std::string(value), stringStyle); } void write(const std::string &key, const char* value, YamlStringStyle stringStyle = YAML_PLAIN_STRING){ write(key, std::string(value), stringStyle); } void write(const std::string &key, bool value); void write(const std::string &key, int value); void write(const std::string &key, double value); void writePath(const std::string &key, const std::string& value); typedef enum { READ_MODE, WRITE_MODE } AssignMode; inline void setAssignMode(AssignMode mode) { this->mode = mode; } template void assign(const std::string& key, T& io_value, const T& defaultValue){ switch(mode){ case READ_MODE: if(!read(key, io_value)){ io_value = defaultValue; } break; case WRITE_MODE: write(key, io_value); break; } } inline iterator begin() { return values.begin(); } inline iterator end() { return values.end(); } inline const_iterator begin() const { return values.begin(); } inline const_iterator end() const { return values.end(); } void throwKeyNotFoundException(const std::string& key) const; private: YamlMapping(const YamlMapping&); YamlMapping& operator=(const YamlMapping&); YamlMapping* openMapping(const std::string& key, bool doOverwrite); YamlMapping* openFlowStyleMapping(const std::string& key, bool doOverwrite); YamlSequence* openSequence(const std::string& key, bool doOverwrite); YamlSequence* openFlowStyleSequence(const std::string& key, bool doOverwrite); inline void insertSub(const std::string& key, YamlNode* node); void writeSub(const std::string &key, const char* text, size_t length, YamlStringStyle stringStyle); static bool compareIters(const YamlMapping::const_iterator& it1, const YamlMapping::const_iterator& it2); Container values; AssignMode mode; int indexCounter; YamlStringStyle keyQuoteStyle; friend class YamlSequence; friend class YamlReaderImpl; friend class YamlWriter; }; typedef boost::intrusive_ptr YamlMappingPtr; /** @todo add 'openMapping' and 'openSequence' methods */ class CNOID_EXPORT YamlSequence : public YamlCollection { typedef std::vector Container; public: YamlSequence(); YamlSequence(int size); ~YamlSequence(); typedef Container::iterator iterator; typedef Container::const_iterator const_iterator; inline bool empty() const { return values.empty(); } inline int size() const { return values.size(); } void clear(); void reserve(int size); inline YamlNode& front() const { return *values.front(); } inline YamlNode& back() const { return *values.back(); } YamlNode* at(int i) const; inline YamlNode& get(int i) const { return *values[i]; } void write(int i, int value); void write(int i, const std::string& value, YamlStringStyle stringStyle = YAML_PLAIN_STRING); bool read(int i, bool &out_value) const; bool read(int i, int &out_value) const; bool read(int i, double &out_value) const; inline YamlNode& operator[](int i) const { return *values[i]; } YamlMapping* newMapping(); inline void append(YamlNodePtr node) { values.push_back(node); } void append(int value); /** @param maxColumns LF is automatically inserted when the column pos is over maxColumsn @param numValues If numValues is not greater than maxColumns, the initial LF is skipped. This feature is disabled if numValues = 0. */ inline void append(int value, int maxColumns, int numValues = 0) { insertLF(maxColumns, numValues); append(value); } void append(size_t value); /** @param maxColumns LF is automatically inserted when the column pos is over maxColumsn @param numValues If numValues is not greater than maxColumns, the initial LF is skipped. This feature is disabled if numValues = 0. */ inline void append(size_t value, int maxColumns, int numValues = 0){ insertLF(maxColumns, numValues); append(value); } void append(double value); /** @param maxColumns LF is automatically inserted when the column pos is over maxColumsn @param numValues If numValues is not greater than maxColumns, the initial LF is skipped. This feature is disabled if numValues = 0. */ inline void append(double value, int maxColumns, int numValues = 0) { insertLF(maxColumns, numValues); append(value); } void append(const std::string& value, YamlStringStyle stringStyle = YAML_PLAIN_STRING); /** @param maxColumns LF is automatically inserted when the column pos is over maxColumsn @param numValues If numValues is not greater than maxColumns, the initial LF is skipped. This feature is disabled if numValues = 0. */ inline void append(const std::string& value, int maxColumns, int numValues = 0, YamlStringStyle stringStyle = YAML_PLAIN_STRING){ insertLF(maxColumns, numValues); append(value, stringStyle); } void appendLF(); inline iterator begin() { return values.begin(); } inline iterator end() { return values.end(); } inline const_iterator begin() const { return values.begin(); } inline const_iterator end() const { return values.end(); }; private: YamlSequence(int line, int column); YamlSequence(int line, int column, int reservedSize); YamlSequence(const YamlSequence&); YamlSequence& operator=(const YamlSequence&); void insertLF(int maxColumns, int numValues); Container values; friend class YamlMapping; friend class YamlReaderImpl; friend class YamlWriter; }; typedef boost::intrusive_ptr YamlSequencePtr; } namespace boost { inline void intrusive_ptr_add_ref(cnoid::YamlNode* obj){ obj->refCounter++; } inline void intrusive_ptr_release(cnoid::YamlNode* obj){ obj->refCounter--; if(obj->refCounter == 0){ delete obj; } } }; #endif choreonoid-1.1.0+dfsg/src/Util/YamlReader.cpp000066400000000000000000000266111207742442300210310ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "YamlReader.h" #include #include #include #include #include using namespace std; using namespace boost; using namespace cnoid; namespace { const bool debugTrace = false; } namespace cnoid { class YamlReaderImpl { public: YamlReaderImpl(); ~YamlReaderImpl(); void setMappingFactory(YamlReader::MappingFactoryBase* factory); void clearDocuments(); bool load(const std::string& filename); bool load_string(const std::string& yamlstring); bool parse(); void popNode(); void addNode(YamlNode* node); void onDocumentStart(yaml_event_t& event); void onDocumentEnd(yaml_event_t& event); void onMappingStart(yaml_event_t& event); void onMappingEnd(yaml_event_t& event); void onSequenceStart(yaml_event_t& event); void onSequenceEnd(yaml_event_t& event); void onScalar(yaml_event_t& event); void onAlias(yaml_event_t& event); static YamlScalar* createScalar(const yaml_event_t& event); yaml_parser_t parser; FILE* file; YamlReader::MappingFactoryBase* mappingFactory; vector documents; int currentDocumentIndex; enum State { NONE, MAPPING_KEY, MAPPING_VALUE, SEQUENCE }; struct NodeInfo { YamlNodePtr node; string key; }; stack nodeStack; bool isRegularMultiSequenceExpected; vector expectedSequenceSizes; string errorMessage; }; } YamlReader::YamlReader() { impl = new YamlReaderImpl(); } YamlReaderImpl::YamlReaderImpl() { yaml_parser_initialize(&parser); file = 0; mappingFactory = new YamlReader::MappingFactory(); currentDocumentIndex = 0; isRegularMultiSequenceExpected = false; } YamlReader::~YamlReader() { delete impl; } YamlReaderImpl::~YamlReaderImpl() { yaml_parser_delete(&parser); if(file){ fclose(file); file = 0; } delete mappingFactory; } void YamlReader::setMappingFactory(MappingFactoryBase* factory) { impl->setMappingFactory(factory); } void YamlReaderImpl::setMappingFactory(YamlReader::MappingFactoryBase* factory) { delete mappingFactory; mappingFactory = factory; } void YamlReader::expectRegularMultiSequence() { impl->isRegularMultiSequenceExpected = true; } void YamlReader::clearDocuments() { impl->clearDocuments(); } void YamlReaderImpl::clearDocuments() { while(!nodeStack.empty()){ nodeStack.pop(); } documents.clear(); } bool YamlReader::load(const std::string& filename) { return impl->load(filename); } bool YamlReaderImpl::load(const std::string& filename) { clearDocuments(); if(isRegularMultiSequenceExpected){ expectedSequenceSizes.clear(); } currentDocumentIndex = 0; bool result = false; FILE* file = fopen(filename.c_str(), "rb"); if(file==NULL){ errorMessage = strerror(errno); } else { yaml_parser_set_input_file(&parser, file); try { result = parse(); } catch(const YamlNode::Exception& ex){ errorMessage = str(format("%1% at line %2%, column %3%") % ex.message() % ex.line() % ex.column()); } fclose(file); } return result; } bool YamlReader::load_string(const std::string& yamlstring) { return impl->load_string(yamlstring); } bool YamlReaderImpl::load_string(const std::string& yamlstring) { clearDocuments(); if(isRegularMultiSequenceExpected){ expectedSequenceSizes.clear(); } currentDocumentIndex = 0; bool result = false; yaml_parser_set_input_string(&parser, (const unsigned char *)(yamlstring.c_str()), yamlstring.length()); try { result = parse(); } catch(const YamlNode::Exception& ex){ errorMessage = str(format("%1% at line %2%, column %3%") % ex.message() % ex.line() % ex.column()); } return result; } bool YamlReaderImpl::parse() { yaml_event_t event; bool done = false; while (!done) { if(!yaml_parser_parse(&parser, &event)){ goto error; } switch(event.type){ case YAML_STREAM_START_EVENT: break; case YAML_STREAM_END_EVENT: done = true; break; case YAML_DOCUMENT_START_EVENT: onDocumentStart(event); break; case YAML_DOCUMENT_END_EVENT: onDocumentEnd(event); break; case YAML_MAPPING_START_EVENT: onMappingStart(event); break; case YAML_MAPPING_END_EVENT: onMappingEnd(event); break; case YAML_SEQUENCE_START_EVENT: onSequenceStart(event); break; case YAML_SEQUENCE_END_EVENT: onSequenceEnd(event); break; case YAML_SCALAR_EVENT: onScalar(event); break; case YAML_ALIAS_EVENT: onAlias(event); break; default: break; } yaml_event_delete(&event); } return !documents.empty(); error: if(debugTrace){ cout << "error" << endl; } if(parser.error != YAML_NO_ERROR && parser.problem != NULL){ YamlNode::Exception ex; ex.setPosition(parser.problem_mark.line, parser.problem_mark.column); ex.setMessage(parser.problem); throw ex; } return false; } void YamlReaderImpl::popNode() { YamlNodePtr current = nodeStack.top().node; nodeStack.pop(); if(nodeStack.empty()){ documents.push_back(current); } else { addNode(current.get()); } } void YamlReaderImpl::addNode(YamlNode* node) { NodeInfo& info = nodeStack.top(); YamlNode* parent = info.node.get(); YamlNodeType type = parent->type(); if(type == YAML_MAPPING){ YamlMapping* mapping = static_cast(parent); mapping->insert(info.key, node); info.key.clear(); } else if(type == YAML_SEQUENCE){ YamlSequence* sequence = static_cast(parent); sequence->append(node); } } void YamlReaderImpl::onDocumentStart(yaml_event_t& event) { if(debugTrace){ cout << "YamlReaderImpl::onDocumentStart()" << endl; } } void YamlReaderImpl::onDocumentEnd(yaml_event_t& event) { if(debugTrace){ cout << "YamlReaderImpl::onDocumentEnd()" << endl; } } void YamlReaderImpl::onMappingStart(yaml_event_t& event) { if(debugTrace){ cout << "YamlReaderImpl::onMappingStart()" << endl; } NodeInfo info; YamlMapping* mapping = mappingFactory->create(event.start_mark.line, event.start_mark.column); mapping->setFlowStyle(event.data.mapping_start.style == YAML_FLOW_MAPPING_STYLE); info.node = mapping; nodeStack.push(info); } void YamlReaderImpl::onMappingEnd(yaml_event_t& event) { if(debugTrace){ cout << "YamlReaderImpl::onMappingEnd()" << endl; } popNode(); } void YamlReaderImpl::onSequenceStart(yaml_event_t& event) { if(debugTrace){ cout << "YamlReaderImpl::onSequenceStart()" << endl; } NodeInfo info; YamlSequence* sequence; if(!isRegularMultiSequenceExpected){ sequence = new YamlSequence(event.start_mark.line, event.start_mark.column); } else { size_t level = nodeStack.size(); if(expectedSequenceSizes.size() <= level){ expectedSequenceSizes.resize(level + 1, 0); } const int prevSize = expectedSequenceSizes[level]; sequence = new YamlSequence(event.start_mark.line, event.start_mark.column, prevSize); } sequence->setFlowStyle(event.data.sequence_start.style == YAML_FLOW_SEQUENCE_STYLE); info.node = sequence; nodeStack.push(info); } void YamlReaderImpl::onSequenceEnd(yaml_event_t& event) { if(debugTrace){ cout << "YamlReaderImpl::onSequenceEnd()" << endl; } if(isRegularMultiSequenceExpected){ YamlSequence* sequence = static_cast(nodeStack.top().node.get()); const int level = nodeStack.size() - 1; expectedSequenceSizes[level] = sequence->size(); } popNode(); } void YamlReaderImpl::onScalar(yaml_event_t& event) { if(debugTrace){ cout << "YamlReaderImpl::onScalar()" << endl; } yaml_char_t* value = event.data.scalar.value; size_t length = event.data.scalar.length; if(nodeStack.empty()){ YamlNode::SyntaxException ex; ex.setMessage("Scalar value cannot be put on the top-level text position"); const yaml_mark_t& start_mark = event.start_mark; ex.setPosition(start_mark.line, start_mark.column); throw ex; } NodeInfo& info = nodeStack.top(); YamlNodePtr& parent = info.node; YamlNodeType type = parent->type(); if(type == YAML_MAPPING){ if(info.key.empty()){ info.key = string((char*)value, length); if(info.key.empty()){ YamlNode::SyntaxException ex; ex.setMessage("empty key"); const yaml_mark_t& start_mark = event.start_mark; ex.setPosition(start_mark.line, start_mark.column); throw ex; } } else { YamlScalar* scalar = createScalar(event); addNode(scalar); } } else if(type == YAML_SEQUENCE){ YamlScalar* scalar = createScalar(event); addNode(scalar); } } YamlScalar* YamlReaderImpl::createScalar(const yaml_event_t& event) { YamlScalar* scalar = new YamlScalar((char*)event.data.scalar.value, event.data.scalar.length); const yaml_mark_t& start_mark = event.start_mark; scalar->line_ = start_mark.line; scalar->column_ = start_mark.column; switch(event.data.scalar.style){ case YAML_PLAIN_SCALAR_STYLE: scalar->stringStyle = YAML_PLAIN_STRING; break; case YAML_SINGLE_QUOTED_SCALAR_STYLE: scalar->stringStyle = YAML_SINGLE_QUOTED; break; case YAML_DOUBLE_QUOTED_SCALAR_STYLE: scalar->stringStyle = YAML_DOUBLE_QUOTED; break; case YAML_LITERAL_SCALAR_STYLE: scalar->stringStyle = YAML_LITERAL; break; case YAML_FOLDED_SCALAR_STYLE: scalar->stringStyle = YAML_FOLDED; break; default: scalar->stringStyle = YAML_DOUBLE_QUOTED; } return scalar; } void YamlReaderImpl::onAlias(yaml_event_t& event) { if(debugTrace){ cout << "YamlReaderImpl::onAlias()" << endl; } } int YamlReader::numDocuments() { return impl->documents.size(); } YamlNode* YamlReader::document(int index) { if(index >= static_cast(impl->documents.size())){ YamlNode::DocumentNotFoundException ex; if(index == 0){ ex.setMessage("The yaml file does not contains any documents."); } else { ex.setMessage(str(format("The yaml file does not contains %1%-th document.") % index)); } ex.setPosition(-1, -1); throw ex; } return impl->documents[index].get(); } const std::string& YamlReader::errorMessage() { return impl->errorMessage; } choreonoid-1.1.0+dfsg/src/Util/YamlReader.h000066400000000000000000000024161207742442300204730ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_YAML_READER_H_INCLUDED #define CNOID_UTIL_YAML_READER_H_INCLUDED #include "YamlNodes.h" #include "exportdecl.h" namespace cnoid { class YamlReaderImpl; class CNOID_EXPORT YamlReader { class MappingFactoryBase { public: virtual YamlMapping* create(int line, int column) = 0; }; template class MappingFactory : public MappingFactoryBase { public: virtual YamlMapping* create(int line, int column) { return new MappingType(line, column); } }; public: YamlReader(); ~YamlReader(); template inline void setMappingClass() { setMappingFactory(new MappingFactory()); } void expectRegularMultiSequence(); bool load(const std::string& filename); bool load_string(const std::string& yamlstring); int numDocuments(); YamlNode* document(int index = 0); void clearDocuments(); const std::string& errorMessage(); private: friend class YamlReaderImpl; YamlReaderImpl* impl; void setMappingFactory(MappingFactoryBase* factory); }; } #endif choreonoid-1.1.0+dfsg/src/Util/YamlWriter.cpp000066400000000000000000000243761207742442300211110ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #include "YamlWriter.h" #include "Utf8.h" #include #include #include using namespace std; using namespace boost; using namespace cnoid; YamlWriter::YamlWriter(const std::string filename) { indentWidth = 2; isCurrentNewLine = true; current = 0; numDocuments = 0; isKeyOrderPreservationMode = false; doubleFormat = "%.7g"; ofs.open(filename.c_str()); pushState(TOP, false); } YamlWriter::~YamlWriter() { ofs.close(); } void YamlWriter::setIndentWidth(int n) { if(isTopLevel()){ indentWidth = n; } } void YamlWriter::setKeyOrderPreservationMode(bool on) { isKeyOrderPreservationMode = true; } bool YamlWriter::isTopLevel() { return (states.size() <= 1); } YamlWriter::State& YamlWriter::pushState(int type, bool isFlowStyle) { bool parentFlowStyle = current ? current->isFlowStyle : isFlowStyle; const int level = std::max(static_cast(states.size() - 1), 0); states.push(State()); State& state = states.top(); state.type = type; state.isFlowStyle = parentFlowStyle ? true : isFlowStyle; state.isKeyPut = false; state.hasValuesBeenPut = false; state.indentString = string(level * indentWidth, ' '); current = &state; return state; } void YamlWriter::popState() { states.pop(); current = &states.top(); } void YamlWriter::newLine() { if(!isCurrentNewLine){ ofs << "\n"; isCurrentNewLine = true; } } void YamlWriter::indent() { if(!isCurrentNewLine){ newLine(); } ofs << current->indentString; } void YamlWriter::startDocument() { newLine(); if(numDocuments > 0){ ofs << "\n"; } ofs << "---\n"; ++numDocuments; } void YamlWriter::putComment(const std::string& comment, bool doNewLine) { if(doNewLine){ indent(); } ofs << "# " << toUtf8(comment); isCurrentNewLine = false; newLine(); } bool YamlWriter::makeValuePutReady() { switch(current->type){ case MAPPING: return current->isKeyPut; case SEQUENCE: if(!current->isFlowStyle){ indent(); ofs << "- "; } isCurrentNewLine = false; return true; default: return true; } } bool YamlWriter::startValuePut() { if(makeValuePutReady()){ if(current->type == SEQUENCE && current->isFlowStyle){ if(current->hasValuesBeenPut){ ofs << ", "; } if(doInsertLineFeed){ newLine(); indent(); doInsertLineFeed = false; isCurrentNewLine = false; } } return true; } return false; } void YamlWriter::endValuePut() { current->hasValuesBeenPut = true; if(current->type == MAPPING){ current->isKeyPut = false; } if(!current->isFlowStyle){ newLine(); } } void YamlWriter::putString_(const std::string& value) { if(startValuePut()){ ofs << value; endValuePut(); } } void YamlWriter::putString(const std::string& value) { putString_(toUtf8(value)); } void YamlWriter::putSingleQuotedString_(const std::string& value) { if(startValuePut()){ ofs << "'" << value << "'"; endValuePut(); } } void YamlWriter::putSingleQuotedString(const std::string& value) { putSingleQuotedString_(toUtf8(value)); } void YamlWriter::putDoubleQuotedString_(const std::string& value) { if(startValuePut()){ ofs << "\"" << value << "\""; endValuePut(); } } void YamlWriter::putDoubleQuotedString(const std::string& value) { putDoubleQuotedString_(toUtf8(value)); } void YamlWriter::putBlockStyleString(const std::string& value, bool isLiteral) { if(current->isFlowStyle){ YamlNode::SyntaxException ex; ex.setMessage("A block-style string cannot be inserted into a flow-style container"); throw ex; } if(startValuePut()){ static char_separator sep("\r", "\n"); typedef tokenizer > Tokenizer; Tokenizer tokens(value, sep); if(isLiteral){ ofs << "|\n"; } else { ofs << ">\n"; } const int level = std::max(static_cast(states.size() - 1), 0); string indentString(level * indentWidth, ' '); ofs << indentString; bool afterLF = false; Tokenizer::iterator it = tokens.begin(); while(it != tokens.end()){ if(afterLF){ ofs << indentString; afterLF = false; } if(*it == "\n"){ if(++it == tokens.end()){ break; } ofs << "\n"; afterLF = true; } else { ofs << toUtf8(*it++); } } endValuePut(); } } void YamlWriter::putScalar(const double& value) { char buf[20]; #ifdef _WIN32 _snprintf(buf, 20, doubleFormat, value); #else snprintf(buf, 20, doubleFormat, value); #endif putString_(buf); } void YamlWriter::setDoubleFormat(const char* format) { doubleFormat = format; } void YamlWriter::startMapping() { startMappingSub(false); } void YamlWriter::startFlowStyleMapping() { startMappingSub(true); } void YamlWriter::startMappingSub(bool isFlowStyle) { if(startValuePut()){ int parentType = current->type; State& state = pushState(MAPPING, isFlowStyle); if(!state.isFlowStyle){ if(parentType == MAPPING){ newLine(); } } else { ofs << "{ "; isCurrentNewLine = false; } } } void YamlWriter::putKey_(const std::string& key, YamlStringStyle style) { if(current->type == MAPPING && !current->isKeyPut){ if(current->isFlowStyle){ if(current->hasValuesBeenPut){ ofs << ", "; } } else { indent(); } switch(style){ case YAML_SINGLE_QUOTED: ofs << "'" << key << "': "; break; case YAML_DOUBLE_QUOTED: ofs << "\"" << key << "\": "; break; default: ofs << key << ": "; break; } current->isKeyPut = true; isCurrentNewLine = false; } } void YamlWriter::putKey(const std::string& key, YamlStringStyle style) { putKey_(toUtf8(key), style); } void YamlWriter::endMapping() { if(current->type == MAPPING){ if(current->isFlowStyle){ ofs << " }"; } popState(); endValuePut(); } } void YamlWriter::startSequence() { startSequenceSub(false); } void YamlWriter::startFlowStyleSequence() { startSequenceSub(true); } void YamlWriter::startSequenceSub(bool isFlowStyle) { if(startValuePut()){ State& state = pushState(SEQUENCE, isFlowStyle); if(!state.isFlowStyle){ if(!isTopLevel()){ newLine(); } } else { ofs << "[ "; isCurrentNewLine = false; doInsertLineFeed = false; } } } void YamlWriter::endSequence() { if(current->type == SEQUENCE){ if(current->isFlowStyle){ ofs << " ]"; } popState(); endValuePut(); } } void YamlWriter::putNode(YamlNode& node) { YamlNodePtr pNode = &node; putNode(pNode); } void YamlWriter::putNode(const YamlNodePtr& node) { switch(node->type()){ case YAML_SCALAR: { const YamlScalar* scalar = static_cast(node.get()); if(scalar->stringStyle == YAML_PLAIN_STRING){ putString_(scalar->stringValue); } else if(scalar->stringStyle == YAML_SINGLE_QUOTED){ putSingleQuotedString_(scalar->stringValue); } else if(scalar->stringStyle == YAML_DOUBLE_QUOTED){ putDoubleQuotedString_(scalar->stringValue); } else if(scalar->stringStyle == YAML_LITERAL){ putLiteralString(scalar->stringValue); } else if(scalar->stringStyle == YAML_FOLDED){ putFoldedString(scalar->stringValue); } else { putDoubleQuotedString_(scalar->stringValue); } } break; case YAML_MAPPING: putMappingNode(static_cast(node.get())); break; case YAML_SEQUENCE: putSequenceNode(static_cast(node.get())); break; case YAML_LF: if(current->isFlowStyle){ doInsertLineFeed = true; } break; default: cout << "hogehoge" << endl; throw "hoge"; } } void YamlWriter::putMappingNode(const YamlMapping* mapping) { if(mapping->isFlowStyle()){ startFlowStyleMapping(); } else { startMapping(); } if(isKeyOrderPreservationMode){ const int n(mapping->size()); vector iters(n); int index = 0; for(YamlMapping::const_iterator it = mapping->begin(); it != mapping->end(); ++it){ iters[index++] = it; } struct KeyOrderCmpFunc { bool operator()(const YamlMapping::const_iterator& it1, const YamlMapping::const_iterator& it2) const { return (it1->second->indexInMapping < it2->second->indexInMapping); } }; std::sort(iters.begin(), iters.end(), &YamlMapping::compareIters); for(int i=0; i < n; ++i){ YamlMapping::const_iterator& it = iters[i]; putKey_(it->first, mapping->keyQuoteStyle); const YamlNodePtr& node = it->second; putNode(node); } } else { for(YamlMapping::const_iterator it = mapping->begin(); it != mapping->end(); ++it){ putKey_(it->first, mapping->keyQuoteStyle); const YamlNodePtr& node = it->second; putNode(node); } } endMapping(); } void YamlWriter::putSequenceNode(const YamlSequence* sequence) { if(sequence->isFlowStyle()){ startFlowStyleSequence(); } else { startSequence(); } const int n = sequence->size(); for(int i=0; i < n; ++i){ putNode(sequence->values[i]); } endSequence(); } choreonoid-1.1.0+dfsg/src/Util/YamlWriter.h000066400000000000000000000064031207742442300205450ustar00rootroot00000000000000/** @author Shin'ichiro Nakaoka */ #ifndef CNOID_UTIL_YAML_WRITER_H_INCLUDED #define CNOID_UTIL_YAML_WRITER_H_INCLUDED #include "YamlNodes.h" #include #include #include #include #include #include "exportdecl.h" namespace cnoid { class CNOID_EXPORT YamlWriter { public: YamlWriter(const std::string filename); ~YamlWriter(); void putNode(YamlNode& node); void putNode(const YamlNodePtr& node); void setIndentWidth(int n); void setKeyOrderPreservationMode(bool on); void startDocument(); void putComment(const std::string& comment, bool doNewLine = true); void putString(const std::string& value); void putSingleQuotedString(const std::string& value); void putDoubleQuotedString(const std::string& value); void putBlockStyleString(const std::string& value, bool isLiteral); inline void putLiteralString(const std::string& value) { putBlockStyleString(value, true); } inline void putFoldedString(const std::string& value) { putBlockStyleString(value, false); } template inline void putScalar(const DataType& value){ putString(boost::lexical_cast(value)); } void putScalar(const double& value); void setDoubleFormat(const char* format); void startMapping(); void startFlowStyleMapping(); void putKey(const std::string& key, YamlStringStyle style = YAML_PLAIN_STRING); template inline void putKeyValue(const std::string& key, const DataType& value){ putKey(key); putScalar(value); } inline void putKeyValue(const std::string& key, const std::string& value){ putKey(key); putDoubleQuotedString(value); } void endMapping(); void startSequence(); void startFlowStyleSequence(); void endSequence(); private: std::ofstream ofs; int indentWidth; bool isCurrentNewLine; int numDocuments; bool isKeyOrderPreservationMode; bool doInsertLineFeed; const char* doubleFormat; enum { TOP, MAPPING, SEQUENCE }; struct State { int type; bool isFlowStyle; bool isKeyPut; bool hasValuesBeenPut; std::string indentString; }; std::stack states; State* current; bool isTopLevel(); State& pushState(int type, bool isFlowStyle); void popState(); void indent(); void newLine(); bool makeValuePutReady(); bool startValuePut(); void endValuePut(); void putString_(const std::string& value); void putSingleQuotedString_(const std::string& value); void putDoubleQuotedString_(const std::string& value); void putKey_(const std::string& key, YamlStringStyle style); void startMappingSub(bool isFlowStyle); void startSequenceSub(bool isFlowStyle); void putMappingNode(const YamlMapping* mapping); void putSequenceNode(const YamlSequence* sequence); }; } #endif choreonoid-1.1.0+dfsg/src/Util/exportdecl.h000066400000000000000000000004461207742442300206200ustar00rootroot00000000000000 #ifdef CNOID_EXPORT #undef CNOID_EXPORT #endif #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) # ifdef CnoidUtil_EXPORTS # define CNOID_EXPORT __declspec(dllexport) # else # define CNOID_EXPORT __declspec(dllimport) # endif #else # define CNOID_EXPORT #endif choreonoid-1.1.0+dfsg/src/Util/gettext.h.in000066400000000000000000000015651207742442300205430ustar00rootroot00000000000000/* This header should not be included in other header files, but included in the most bottom position of the inclusion part in the implementation (.cpp) files where the message internationalization (texts with _("...") form) is required. */ #ifdef CNOID_GETTEXT_DOMAIN_NAME #undef CNOID_GETTEXT_DOMAIN_NAME #endif #define CNOID_GETTEXT_DOMAIN_NAME "@target@-@CNOID_VERSION@" #ifdef _ #undef _ #endif #cmakedefine01 CNOID_ENABLE_GETTEXT #if CNOID_ENABLE_GETTEXT #include "libintl.h" #define _(text) dgettext(CNOID_GETTEXT_DOMAIN_NAME, text) #define N_(string) string #else namespace cnoid { inline const char* bindtextdomain(const char* domainname, const char* dirname) { return dirname; } inline const char* dgettext(const char* domainname, const char* msgid){ return msgid; } } #define _(string) string #define N_(string) string #endif